Doxygen Source Code Documentation

Main Page Alphabetical List Data Structures File List Data Fields Globals Search
rotmat.c

00001 #define V2PI 6.2831853071795864
00002 #define VPI  3.1415926535897932
00003 #define VHPI 1.5707963267948966
00004 #define VEPS 0.00001
00005 
00006 /*! Compute rotation matrix about axis (a,b,c) of angle th radians. */
00007 
00008 static THD_dmat33 RCREND_axis_rotmatrix( double th, double a,double b,double c )
00009 {
00010   THD_dmat33 ips,ims,ss,id , rr ;
00011   double na,nb,nc, nn , tp ;
00012 
00013   LOAD_DIAG_DMAT(id,1,1,1) ;             /* identity matrix */
00014   nn = sqrt(a*a + b*b + c*c) ;           /* norm of axis vector */
00015   tp = 0.5 * (th-floor(th/V2PI)*V2PI) ;  /* half of angle reduced to (0..2*Pi) */
00016 
00017   /* if axis norm is zero, or angle is zero, return identity matrix */
00018 
00019   if( nn < VEPS || fabs(tp) < VEPS || fabs(tp-VPI) < VEPS ) return id ;
00020 
00021   /* if angle is nearly Pi, then use special formula */
00022 
00023   if( fabs(tp-VHPI) < VEPS ){
00024     na = a/nn ; nb = b/nn ; nc = c/nn ;
00025     LOAD_DMAT(rr , na*na-nb*nb-nc*nc, 2.0*na*nb        , 2.0*na*nc        ,
00026                    2.0*na*nb        , nb*nb-na*na-nc*nc, 2.0*nb*nc        ,
00027                    2.0*na*nc        , 2.0*nb*nc        , nc*nc-na*na-nb*nb ) ;
00028     return rr ;
00029   }
00030 
00031   /* otherwise, rr = [I-S] * inv[I+S],                  [  0   nc -nb ]
00032      where skew-symmetric S is given by  S = tan(th/2)* [ -nc  0   na ]
00033                                                         [  nb -na  0  ]  */
00034 
00035   ss.mat[0][0] = ss.mat[1][1] = ss.mat[2][2] = 0.0 ;
00036   nn = tan(tp)/nn ;
00037   na = a*nn ; ss.mat[1][2] =  na ; ss.mat[2][1] = -na ;
00038   nb = b*nn ; ss.mat[0][2] = -nb ; ss.mat[2][0] =  nb ;
00039   nc = c*nn ; ss.mat[0][1] =  nc ; ss.mat[1][0] = -nc ;
00040   ims = SUB_DMAT(id,ss) ;
00041   ips = ADD_DMAT(id,ss) ; ss = DMAT_INV(ips) ;
00042   rr  = DMAT_MUL(ims,ss) ;
00043 
00044   return rr ;
00045 }
AFNI/NIfTI Server

Sections

Personal tools

Navigation

Doxygen Source Code Documentation

rotmat.c
