Doxygen Source Code Documentation

Main Page Alphabetical List Data Structures File List Data Fields Globals Search
vecmat.h

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 #ifndef _MCW_3DVECMAT_
00008 #define _MCW_3DVECMAT_
00009 
00010 /*-------------------------------------------------------------------
00011    06 Feb 2001: modified to remove FLOAT_TYPE, and to create
00012                 separate float and double data types and macros
00013 ---------------------------------------------------------------------*/
00014 
00015 #include <math.h>
00016 
00017 /*-------------------------------------------------------------------*/
00018 /*-----             3-vector and matrix structures              -----*/
00019 /*----- Double precision versions exist at bottom of this file. -----*/
00020 
00021 /*! 3-vector of integers. */
00022 
00023 typedef struct { int ijk[3] ;      } THD_ivec3 ;
00024 
00025 /*! 3-vector of floats. */
00026 
00027 typedef struct { float xyz[3] ;    } THD_fvec3 ;
00028 
00029 /*! 3x3 matrix of floats. */
00030 
00031 typedef struct { float mat[3][3] ; } THD_mat33 ;
00032 
00033 /*! 3x3 matrix and a 3-vector (basically an affine transform). */
00034 
00035 typedef struct {
00036    THD_fvec3 vv ; /*!< the vector */
00037    THD_mat33 mm ; /*!< the matrix */
00038 } THD_vecmat ;
00039 
00040 /*-------------------------------------------------------------------*/
00041 /*-----       macros that operate on 3 vectors and matrices     -----*/
00042 
00043 /*! Load THD_ivec3 iv with 3 integers. */
00044 
00045 #define LOAD_IVEC3(iv,i,j,k) ( (iv).ijk[0]=(i), \
00046                                (iv).ijk[1]=(j), \
00047                                (iv).ijk[2]=(k)    )
00048 
00049 /*! Take 3 integers out of THD_ivec3 iv. */
00050 
00051 #define UNLOAD_IVEC3(iv,i,j,k) ( (i)=(iv).ijk[0], \
00052                                  (j)=(iv).ijk[1], \
00053                                  (k)=(iv).ijk[2]   )
00054 
00055 /*! Load THD_fvec3 fv with 3 floats. */
00056 
00057 #define LOAD_FVEC3(fv,x,y,z) ( (fv).xyz[0]=(x), \
00058                                (fv).xyz[1]=(y), \
00059                                (fv).xyz[2]=(z)   )
00060 
00061 /*! Take 3 floats out of THD_fvec3 fv. */
00062 
00063 #define UNLOAD_FVEC3(fv,x,y,z) ( (x)=(fv).xyz[0], \
00064                                  (y)=(fv).xyz[1], \
00065                                  (z)=(fv).xyz[2]   )
00066 
00067 static THD_ivec3 tempA_ivec3 , tempB_ivec3 ;  /* temps for macros below */
00068 static THD_fvec3 tempA_fvec3 , tempB_fvec3 ;
00069 static THD_mat33 tempA_mat33 , tempB_mat33 ;
00070 static float tempRWC ;
00071 
00072 /*! Return a temporary THD_ivec3 from 3 integers. */
00073 
00074 #define TEMP_IVEC3(i,j,k) ( tempB_ivec3.ijk[0]=(i), \
00075                             tempB_ivec3.ijk[1]=(j), \
00076                             tempB_ivec3.ijk[2]=(k), tempB_ivec3 )
00077 
00078 /*! Return a temporary THD_fvec3 from 3 floats. */
00079 
00080 #define TEMP_FVEC3(x,y,z) ( tempB_fvec3.xyz[0]=(x), \
00081                             tempB_fvec3.xyz[1]=(y), \
00082                             tempB_fvec3.xyz[2]=(z), tempB_fvec3 )
00083 
00084 /*! Debug printout of a THD_ivec3. */
00085 
00086 #define DUMP_IVEC3(str,iv) \
00087    fprintf(stderr,"%s: %d %d %d\n",(str),(iv).ijk[0],(iv).ijk[1],(iv).ijk[2])
00088 
00089 /*! Debug printout of a THD_fvec3. */
00090 
00091 #define DUMP_FVEC3(str,fv) \
00092    fprintf(stderr,"%s: %13.6g %13.6g %13.6g\n",(str),(fv).xyz[0],(fv).xyz[1],(fv).xyz[2])
00093 
00094 /*! Debug printout of a THD_mat33. */
00095 
00096 #define DUMP_MAT33(str,A)                                  \
00097    fprintf(stderr,                                         \
00098           "%10.10s: [ %13.6g %13.6g %13.6g ]\n"            \
00099        "            [ %13.6g %13.6g %13.6g ]\n"            \
00100        "            [ %13.6g %13.6g %13.6g ]\n" ,          \
00101      str , (A).mat[0][0] , (A).mat[0][1] , (A).mat[0][2] , \
00102            (A).mat[1][0] , (A).mat[1][1] , (A).mat[1][2] , \
00103            (A).mat[2][0] , (A).mat[2][1] , (A).mat[2][2]  )
00104 
00105 /*! Debug printout of a THD_vecmat. */
00106 
00107 #define DUMP_VECMAT(str,vvmm) ( DUMP_MAT33(str,vvmm.mm) , DUMP_FVEC3(str,vvmm.vv) )
00108 
00109 /*--- macros for operations on floating 3 vectors,
00110       with heavy use of the comma operator and structure assignment! ---*/
00111 
00112   /*! Return negation of THD_fvec3 a. */
00113 
00114 #define NEGATE_FVEC3(a) ( (a).xyz[0] = -(a).xyz[0] , \
00115                           (a).xyz[1] = -(a).xyz[1] , \
00116                           (a).xyz[2] = -(a).xyz[2]    )
00117 
00118   /*! Return THD_fvec3 a-b. */
00119 
00120 #define SUB_FVEC3(a,b) \
00121    ( tempA_fvec3.xyz[0] = (a).xyz[0] - (b).xyz[0] , \
00122      tempA_fvec3.xyz[1] = (a).xyz[1] - (b).xyz[1] , \
00123      tempA_fvec3.xyz[2] = (a).xyz[2] - (b).xyz[2] , tempA_fvec3 )
00124 
00125   /*! Return THD_fvec3 a+b. */
00126 
00127 #define ADD_FVEC3(a,b) \
00128    ( tempA_fvec3.xyz[0] = (a).xyz[0] + (b).xyz[0] , \
00129      tempA_fvec3.xyz[1] = (a).xyz[1] + (b).xyz[1] , \
00130      tempA_fvec3.xyz[2] = (a).xyz[2] + (b).xyz[2] , tempA_fvec3 )
00131 
00132   /*! Return THD_fvec3 a/|a| (unit vector). */
00133 
00134 #define NORMALIZE_FVEC3(a) \
00135    ( tempRWC =   (a).xyz[0] * (a).xyz[0]                 \
00136                + (a).xyz[1] * (a).xyz[1]                 \
00137                + (a).xyz[2] * (a).xyz[2]               , \
00138      tempRWC = (tempRWC > 0) ? (1.0/sqrt(tempRWC)) : 0 , \
00139      tempA_fvec3.xyz[0] = (a).xyz[0] * tempRWC         , \
00140      tempA_fvec3.xyz[1] = (a).xyz[1] * tempRWC         , \
00141      tempA_fvec3.xyz[2] = (a).xyz[2] * tempRWC         , tempA_fvec3 )
00142 
00143   /*! Return THD_fvec3 a X b (cross product). */
00144 
00145 #define CROSS_FVEC3(a,b) \
00146   ( tempA_fvec3.xyz[0] = (a).xyz[1]*(b).xyz[2] - (a).xyz[2]*(b).xyz[1] , \
00147     tempA_fvec3.xyz[1] = (a).xyz[2]*(b).xyz[0] - (a).xyz[0]*(b).xyz[2] , \
00148     tempA_fvec3.xyz[2] = (a).xyz[0]*(b).xyz[1] - (a).xyz[1]*(b).xyz[0] , \
00149     tempA_fvec3 )
00150 
00151   /*! Return float L2norm(a) from a THD_fvec3. */
00152 
00153 #define SIZE_FVEC3(a) \
00154    sqrt((a).xyz[0]*(a).xyz[0]+(a).xyz[1]*(a).xyz[1]+(a).xyz[2]*(a).xyz[2])
00155 
00156   /*! Return float a dot b from THD_fvec3 inputs. */
00157 
00158 #define DOT_FVEC3(a,b) \
00159    ((a).xyz[0]*(b).xyz[0] + (a).xyz[1]*(b).xyz[1] + (a).xyz[2]*(b).xyz[2])
00160 
00161   /* scale and add two vectors: fa * a + fb * b */
00162 
00163 #define SCLADD_FVEC3(fa,a,fb,b) \
00164   ( tempA_fvec3.xyz[0] = (fa)*(a).xyz[0] + (fb)*(b).xyz[0] , \
00165     tempA_fvec3.xyz[1] = (fa)*(a).xyz[1] + (fb)*(b).xyz[1] , \
00166     tempA_fvec3.xyz[2] = (fa)*(a).xyz[2] + (fb)*(b).xyz[2] , tempA_fvec3 )
00167 
00168   /* round to an integer vector (assume non-negative) */
00169 
00170 #define INT_FVEC3(a) \
00171   ( tempA_ivec3.ijk[0] = (a).xyz[0] + 0.5 , \
00172     tempA_ivec3.ijk[1] = (a).xyz[1] + 0.5 , \
00173     tempA_ivec3.ijk[1] = (a).xyz[2] + 0.5 , tempA_ivec3 ) ;
00174 
00175 #define SIZE_MAT(A)                                                 \
00176   ( fabs((A).mat[0][0]) + fabs((A).mat[0][1]) + fabs((A).mat[0][2])  \
00177    +fabs((A).mat[1][0]) + fabs((A).mat[1][1]) + fabs((A).mat[1][2])   \
00178    +fabs((A).mat[2][0]) + fabs((A).mat[2][1]) + fabs((A).mat[2][2]) )
00179 
00180   /* matrix-vector multiply */
00181 
00182 #define MATVEC(A,x) \
00183   ( tempA_fvec3.xyz[0] = (A).mat[0][0] * (x).xyz[0]  \
00184                         +(A).mat[0][1] * (x).xyz[1]  \
00185                         +(A).mat[0][2] * (x).xyz[2] ,\
00186     tempA_fvec3.xyz[1] = (A).mat[1][0] * (x).xyz[0]  \
00187                         +(A).mat[1][1] * (x).xyz[1]  \
00188                         +(A).mat[1][2] * (x).xyz[2] ,\
00189     tempA_fvec3.xyz[2] = (A).mat[2][0] * (x).xyz[0]  \
00190                         +(A).mat[2][1] * (x).xyz[1]  \
00191                         +(A).mat[2][2] * (x).xyz[2] ,  tempA_fvec3 )
00192 
00193   /* matrix-vector multiply with subtract: output = A (x-b) */
00194 
00195 #define VECSUB_MAT(A,x,b) \
00196   ( tempA_fvec3.xyz[0] = (A).mat[0][0] * ((x).xyz[0]-(b).xyz[0])  \
00197                         +(A).mat[0][1] * ((x).xyz[1]-(b).xyz[1])  \
00198                         +(A).mat[0][2] * ((x).xyz[2]-(b).xyz[2]) ,\
00199     tempA_fvec3.xyz[1] = (A).mat[1][0] * ((x).xyz[0]-(b).xyz[0])  \
00200                         +(A).mat[1][1] * ((x).xyz[1]-(b).xyz[1])  \
00201                         +(A).mat[1][2] * ((x).xyz[2]-(b).xyz[2]) ,\
00202     tempA_fvec3.xyz[2] = (A).mat[2][0] * ((x).xyz[0]-(b).xyz[0])  \
00203                         +(A).mat[2][1] * ((x).xyz[1]-(b).xyz[1])  \
00204                         +(A).mat[2][2] * ((x).xyz[2]-(b).xyz[2]) ,\
00205     tempA_fvec3 )
00206 
00207    /* matrix vector multiply with subtract after: A x - b */
00208 
00209 #define MATVEC_SUB(A,x,b) \
00210   ( tempA_fvec3.xyz[0] = (A).mat[0][0] * (x).xyz[0]               \
00211                         +(A).mat[0][1] * (x).xyz[1]               \
00212                         +(A).mat[0][2] * (x).xyz[2] - (b).xyz[0] ,\
00213     tempA_fvec3.xyz[1] = (A).mat[1][0] * (x).xyz[0]               \
00214                         +(A).mat[1][1] * (x).xyz[1]               \
00215                         +(A).mat[1][2] * (x).xyz[2] - (b).xyz[1] ,\
00216     tempA_fvec3.xyz[2] = (A).mat[2][0] * (x).xyz[0]               \
00217                         +(A).mat[2][1] * (x).xyz[1]               \
00218                         +(A).mat[2][2] * (x).xyz[2] - (b).xyz[2] ,\
00219     tempA_fvec3 )
00220 
00221    /* matrix vector multiply with add after: A x + b */
00222 
00223 #define MATVEC_ADD(A,x,b) \
00224   ( tempA_fvec3.xyz[0] = (A).mat[0][0] * (x).xyz[0]               \
00225                         +(A).mat[0][1] * (x).xyz[1]               \
00226                         +(A).mat[0][2] * (x).xyz[2] + (b).xyz[0] ,\
00227     tempA_fvec3.xyz[1] = (A).mat[1][0] * (x).xyz[0]               \
00228                         +(A).mat[1][1] * (x).xyz[1]               \
00229                         +(A).mat[1][2] * (x).xyz[2] + (b).xyz[1] ,\
00230     tempA_fvec3.xyz[2] = (A).mat[2][0] * (x).xyz[0]               \
00231                         +(A).mat[2][1] * (x).xyz[1]               \
00232                         +(A).mat[2][2] * (x).xyz[2] + (b).xyz[2] ,\
00233     tempA_fvec3 )
00234 
00235   /* matrix-matrix multiply */
00236 
00237 #define ROW_DOT_COL(A,B,i,j) (  (A).mat[i][0] * (B).mat[0][j] \
00238                               + (A).mat[i][1] * (B).mat[1][j] \
00239                               + (A).mat[i][2] * (B).mat[2][j]   )
00240 
00241 #define MAT_MUL(A,B)                                   \
00242   ( tempA_mat33.mat[0][0] = ROW_DOT_COL((A),(B),0,0) , \
00243     tempA_mat33.mat[1][0] = ROW_DOT_COL((A),(B),1,0) , \
00244     tempA_mat33.mat[2][0] = ROW_DOT_COL((A),(B),2,0) , \
00245     tempA_mat33.mat[0][1] = ROW_DOT_COL((A),(B),0,1) , \
00246     tempA_mat33.mat[1][1] = ROW_DOT_COL((A),(B),1,1) , \
00247     tempA_mat33.mat[2][1] = ROW_DOT_COL((A),(B),2,1) , \
00248     tempA_mat33.mat[0][2] = ROW_DOT_COL((A),(B),0,2) , \
00249     tempA_mat33.mat[1][2] = ROW_DOT_COL((A),(B),1,2) , \
00250     tempA_mat33.mat[2][2] = ROW_DOT_COL((A),(B),2,2) , tempA_mat33 )
00251 
00252 #define MAT_SCALAR(A,c)                          \
00253   ( tempA_mat33.mat[0][0] = (c)*(A).mat[0][0] ,  \
00254     tempA_mat33.mat[1][0] = (c)*(A).mat[1][0] ,  \
00255     tempA_mat33.mat[2][0] = (c)*(A).mat[2][0] ,  \
00256     tempA_mat33.mat[0][1] = (c)*(A).mat[0][1] ,  \
00257     tempA_mat33.mat[1][1] = (c)*(A).mat[1][1] ,  \
00258     tempA_mat33.mat[2][1] = (c)*(A).mat[2][1] ,  \
00259     tempA_mat33.mat[0][2] = (c)*(A).mat[0][2] ,  \
00260     tempA_mat33.mat[1][2] = (c)*(A).mat[1][2] ,  \
00261     tempA_mat33.mat[2][2] = (c)*(A).mat[2][2] , tempA_mat33 )
00262 
00263    /* matrix determinant */
00264 
00265 #define MAT_DET(A) \
00266  (  (A).mat[0][0]*(A).mat[1][1]*(A).mat[2][2] \
00267   - (A).mat[0][0]*(A).mat[1][2]*(A).mat[2][1] \
00268   - (A).mat[1][0]*(A).mat[0][1]*(A).mat[2][2] \
00269   + (A).mat[1][0]*(A).mat[0][2]*(A).mat[2][1] \
00270   + (A).mat[2][0]*(A).mat[0][1]*(A).mat[1][2] \
00271   - (A).mat[2][0]*(A).mat[0][2]*(A).mat[1][1]   )
00272 
00273    /* matrix norm [28 Aug 2002] */
00274 
00275 #define MAT_FNORM(A)                 \
00276  sqrt( (A).mat[0][0]*(A).mat[0][0] + \
00277        (A).mat[0][1]*(A).mat[0][1] + \
00278        (A).mat[0][2]*(A).mat[0][2] + \
00279        (A).mat[1][0]*(A).mat[1][0] + \
00280        (A).mat[1][1]*(A).mat[1][1] + \
00281        (A).mat[1][2]*(A).mat[1][2] + \
00282        (A).mat[2][0]*(A).mat[2][0] + \
00283        (A).mat[2][1]*(A).mat[2][1] + \
00284        (A).mat[2][2]*(A).mat[2][2]  )
00285 
00286    /* matrix trace [5 Oct 1998] */
00287 
00288 #define MAT_TRACE(A) ( (A).mat[0][0] + (A).mat[1][1] + (A).mat[2][2] )
00289 
00290    /* matrix inverse */
00291 
00292 #define MAT_INV(A) \
00293  ( tempRWC = 1.0 / MAT_DET(A) , \
00294     tempA_mat33.mat[1][1] = \
00295      ( (A).mat[0][0]*(A).mat[2][2] - (A).mat[0][2]*(A).mat[2][0]) * tempRWC,\
00296     tempA_mat33.mat[2][2] = \
00297      ( (A).mat[0][0]*(A).mat[1][1] - (A).mat[0][1]*(A).mat[1][0]) * tempRWC,\
00298     tempA_mat33.mat[2][0] = \
00299      ( (A).mat[1][0]*(A).mat[2][1] - (A).mat[1][1]*(A).mat[2][0]) * tempRWC,\
00300     tempA_mat33.mat[1][2] = \
00301      (-(A).mat[0][0]*(A).mat[1][2] + (A).mat[0][2]*(A).mat[1][0]) * tempRWC,\
00302     tempA_mat33.mat[0][1] = \
00303      (-(A).mat[0][1]*(A).mat[2][2] + (A).mat[0][2]*(A).mat[2][1]) * tempRWC,\
00304     tempA_mat33.mat[0][0] = \
00305      ( (A).mat[1][1]*(A).mat[2][2] - (A).mat[1][2]*(A).mat[2][1]) * tempRWC,\
00306     tempA_mat33.mat[2][1] = \
00307      (-(A).mat[0][0]*(A).mat[2][1] + (A).mat[0][1]*(A).mat[2][0]) * tempRWC,\
00308     tempA_mat33.mat[1][0] = \
00309      (-(A).mat[1][0]*(A).mat[2][2] + (A).mat[1][2]*(A).mat[2][0]) * tempRWC,\
00310     tempA_mat33.mat[0][2] = \
00311      ( (A).mat[0][1]*(A).mat[1][2] - (A).mat[0][2]*(A).mat[1][1]) * tempRWC,\
00312     tempA_mat33 )
00313 
00314   /* load a matrix from scalars [3 Oct 1998] */
00315 
00316 #define LOAD_MAT(A,a11,a12,a13,a21,a22,a23,a31,a32,a33) \
00317  ( (A).mat[0][0] = (a11) , (A).mat[0][1] = (a12) ,      \
00318    (A).mat[0][2] = (a13) , (A).mat[1][0] = (a21) ,      \
00319    (A).mat[1][1] = (a22) , (A).mat[1][2] = (a23) ,      \
00320    (A).mat[2][0] = (a31) , (A).mat[2][1] = (a32) , (A).mat[2][2] = (a33) )
00321 
00322   /* unload a matrix into scalars [3 Oct 1998] */
00323 
00324 #define UNLOAD_MAT(A,a11,a12,a13,a21,a22,a23,a31,a32,a33) \
00325  ( (a11) = (A).mat[0][0] , (a12) = (A).mat[0][1] ,        \
00326    (a13) = (A).mat[0][2] , (a21) = (A).mat[1][0] ,        \
00327    (a22) = (A).mat[1][1] , (a23) = (A).mat[1][2] ,        \
00328    (a31) = (A).mat[2][0] , (a32) = (A).mat[2][1] , (a33) = (A).mat[2][2] )
00329 
00330    /* diagonal matrix */
00331 
00332 #define LOAD_DIAG_MAT(A,x,y,z) \
00333  ( (A).mat[0][0] = (x) , \
00334    (A).mat[1][1] = (y) , \
00335    (A).mat[2][2] = (z) , \
00336    (A).mat[0][1] = (A).mat[0][2] = (A).mat[1][0] = \
00337    (A).mat[1][2] = (A).mat[2][0] = (A).mat[2][1] = 0.0 )
00338 
00339    /* zero matrix */
00340 
00341 #define LOAD_ZERO_MAT(A) LOAD_DIAG_MAT((A),0,0,0)
00342 
00343    /* elementary rotation matrices:
00344       rotate about axis #ff, from axis #aa toward #bb,
00345       where ff, aa, and bb are a permutation of {0,1,2} */
00346 
00347 #define LOAD_ROTGEN_MAT(A,th,ff,aa,bb)             \
00348  ( (A).mat[aa][aa] = (A).mat[bb][bb] = cos((th)) , \
00349    (A).mat[aa][bb] = sin((th)) ,                   \
00350    (A).mat[bb][aa] = -(A).mat[aa][bb] ,            \
00351    (A).mat[ff][ff] = 1.0 ,                         \
00352    (A).mat[aa][ff] = (A).mat[bb][ff] = (A).mat[ff][aa] = (A).mat[ff][bb] = 0.0 )
00353 
00354 #define LOAD_ROTX_MAT(A,th) LOAD_ROTGEN_MAT(A,th,0,1,2)
00355 #define LOAD_ROTY_MAT(A,th) LOAD_ROTGEN_MAT(A,th,1,2,0)
00356 #define LOAD_ROTZ_MAT(A,th) LOAD_ROTGEN_MAT(A,th,2,0,1)
00357 
00358 #define LOAD_ROT_MAT(A,th,i)                    \
00359   do{ switch( (i) ){                            \
00360         case 0: LOAD_ROTX_MAT(A,th)   ; break ; \
00361         case 1: LOAD_ROTY_MAT(A,th)   ; break ; \
00362         case 2: LOAD_ROTZ_MAT(A,th)   ; break ; \
00363        default: LOAD_DIAG_MAT(A,1,1,1); break ; \
00364       } } while(0)
00365 
00366    /* shear matrices [3 Oct 1998] */
00367 
00368 #define LOAD_SHEARX_MAT(A,f,b,c) ( LOAD_DIAG_MAT(A,(f),1,1) , \
00369                                    (A).mat[0][1] = (b) , (A).mat[0][2] = (c) )
00370 
00371 #define LOAD_SHEARY_MAT(A,f,a,c) ( LOAD_DIAG_MAT(A,1,(f),1) , \
00372                                    (A).mat[1][0] = (a) , (A).mat[1][2] = (c) )
00373 
00374 #define LOAD_SHEARZ_MAT(A,f,a,b) ( LOAD_DIAG_MAT(A,1,1,(f)) , \
00375                                    (A).mat[2][0] = (a) , (A).mat[2][1] = (b) )
00376 
00377    /* matrix transpose */
00378 
00379 #define TRANSPOSE_MAT(A) \
00380  ( tempA_mat33.mat[0][0] = (A).mat[0][0] , \
00381    tempA_mat33.mat[1][0] = (A).mat[0][1] , \
00382    tempA_mat33.mat[2][0] = (A).mat[0][2] , \
00383    tempA_mat33.mat[0][1] = (A).mat[1][0] , \
00384    tempA_mat33.mat[1][1] = (A).mat[1][1] , \
00385    tempA_mat33.mat[2][1] = (A).mat[1][2] , \
00386    tempA_mat33.mat[0][2] = (A).mat[2][0] , \
00387    tempA_mat33.mat[1][2] = (A).mat[2][1] , \
00388    tempA_mat33.mat[2][2] = (A).mat[2][2] , tempA_mat33 )
00389 
00390    /* matrix add & subtract */
00391 
00392 #define ADD_MAT(A,B)                                       \
00393  ( tempA_mat33.mat[0][0] = (A).mat[0][0] + (B).mat[0][0] , \
00394    tempA_mat33.mat[1][0] = (A).mat[1][0] + (B).mat[1][0] , \
00395    tempA_mat33.mat[2][0] = (A).mat[2][0] + (B).mat[2][0] , \
00396    tempA_mat33.mat[0][1] = (A).mat[0][1] + (B).mat[0][1] , \
00397    tempA_mat33.mat[1][1] = (A).mat[1][1] + (B).mat[1][1] , \
00398    tempA_mat33.mat[2][1] = (A).mat[2][1] + (B).mat[2][1] , \
00399    tempA_mat33.mat[0][2] = (A).mat[0][2] + (B).mat[0][2] , \
00400    tempA_mat33.mat[1][2] = (A).mat[1][2] + (B).mat[1][2] , \
00401    tempA_mat33.mat[2][2] = (A).mat[2][2] + (B).mat[2][2] , tempA_mat33 )
00402 
00403 #define SUB_MAT(A,B)                                       \
00404  ( tempA_mat33.mat[0][0] = (A).mat[0][0] - (B).mat[0][0] , \
00405    tempA_mat33.mat[1][0] = (A).mat[1][0] - (B).mat[1][0] , \
00406    tempA_mat33.mat[2][0] = (A).mat[2][0] - (B).mat[2][0] , \
00407    tempA_mat33.mat[0][1] = (A).mat[0][1] - (B).mat[0][1] , \
00408    tempA_mat33.mat[1][1] = (A).mat[1][1] - (B).mat[1][1] , \
00409    tempA_mat33.mat[2][1] = (A).mat[2][1] - (B).mat[2][1] , \
00410    tempA_mat33.mat[0][2] = (A).mat[0][2] - (B).mat[0][2] , \
00411    tempA_mat33.mat[1][2] = (A).mat[1][2] - (B).mat[1][2] , \
00412    tempA_mat33.mat[2][2] = (A).mat[2][2] - (B).mat[2][2] , tempA_mat33 )
00413 
00414    /* component-wise min and max of 3-vectors */
00415 
00416 #define MAX_FVEC3(a,b) \
00417 ( tempA_fvec3.xyz[0] = (((a).xyz[0] > (b).xyz[0]) ? (a).xyz[0] : (b).xyz[0]) ,\
00418   tempA_fvec3.xyz[1] = (((a).xyz[1] > (b).xyz[1]) ? (a).xyz[1] : (b).xyz[1]) ,\
00419   tempA_fvec3.xyz[2] = (((a).xyz[2] > (b).xyz[2]) ? (a).xyz[2] : (b).xyz[2]) ,\
00420   tempA_fvec3 )
00421 
00422 #define MIN_FVEC3(a,b) \
00423 ( tempA_fvec3.xyz[0] = (((a).xyz[0] < (b).xyz[0]) ? (a).xyz[0] : (b).xyz[0]) ,\
00424   tempA_fvec3.xyz[1] = (((a).xyz[1] < (b).xyz[1]) ? (a).xyz[1] : (b).xyz[1]) ,\
00425   tempA_fvec3.xyz[2] = (((a).xyz[2] < (b).xyz[2]) ? (a).xyz[2] : (b).xyz[2]) ,\
00426   tempA_fvec3 )
00427 
00428 /*-------------------------------------------------------------------*/
00429 /*-----         double precision versions of the above          -----*/
00430 
00431 typedef struct { double xyz[3] ; }    THD_dfvec3 ;
00432 typedef struct { double mat[3][3] ; } THD_dmat33 ;
00433 
00434 typedef struct {    /* 3x3 matrix + 3-vector [16 Jul 2000] */
00435    THD_dfvec3 vv ;
00436    THD_dmat33 mm ;
00437 } THD_dvecmat ;
00438 
00439 #define LOAD_DFVEC3(fv,x,y,z) ( (fv).xyz[0]=(x), \
00440                                (fv).xyz[1]=(y), \
00441                                (fv).xyz[2]=(z)   )
00442 
00443 #define UNLOAD_DFVEC3(fv,x,y,z) ( (x)=(fv).xyz[0], \
00444                                  (y)=(fv).xyz[1], \
00445                                  (z)=(fv).xyz[2]   )
00446 
00447 #define DFVEC3_TO_FVEC3(dv,fv) LOAD_FVEC3(fv,dv.xyz[0],dv.xyz[1],dv.xyz[2])
00448 #define FVEC3_TO_DFVEC3(fv,dv) LOAD_DFVEC3(dv,fv.xyz[0],fv.xyz[1],fv.xyz[2])
00449 
00450 static THD_dfvec3 tempA_dfvec3 , tempB_dfvec3 ;
00451 static THD_dmat33 tempA_dmat33 , tempB_dmat33 ;
00452 static double dtempRWC ;
00453 
00454 #define TEMP_DFVEC3(x,y,z) ( tempB_dfvec3.xyz[0]=(x), \
00455                             tempB_dfvec3.xyz[1]=(y), \
00456                             tempB_dfvec3.xyz[2]=(z), tempB_dfvec3 )
00457 
00458 #define DUMP_DFVEC3(str,fv) \
00459    fprintf(stderr,"%s: %13.6g %13.6g %13.6g\n",(str),(fv).xyz[0],(fv).xyz[1],(fv).xyz[2])
00460 
00461 #define DUMP_DMAT33(str,A)                                  \
00462    fprintf(stderr,                                         \
00463           "%10.10s: [ %13.6g %13.6g %13.6g ]\n"            \
00464        "            [ %13.6g %13.6g %13.6g ]\n"            \
00465        "            [ %13.6g %13.6g %13.6g ]\n" ,          \
00466      str , (A).mat[0][0] , (A).mat[0][1] , (A).mat[0][2] , \
00467            (A).mat[1][0] , (A).mat[1][1] , (A).mat[1][2] , \
00468            (A).mat[2][0] , (A).mat[2][1] , (A).mat[2][2]  )
00469 
00470 #define DUMP_DVECMAT(str,vvmm) ( DUMP_DMAT33(str,vvmm.mm) , DUMP_DFVEC3(str,vvmm.vv) )
00471 
00472   /*! Convert from dmat33 to mat33 (double to single precision) */
00473 
00474 #define DMAT_TO_MAT(D,M)                                    \
00475  LOAD_MAT(M,(D).mat[0][0] , (D).mat[0][1] , (D).mat[0][2] , \
00476             (D).mat[1][0] , (D).mat[1][1] , (D).mat[1][2] , \
00477             (D).mat[2][0] , (D).mat[2][1] , (D).mat[2][2]  )
00478 
00479 /*--- macros for operations on floating 3 vectors,
00480       with heavy use of the comma operator and structure assignment! ---*/
00481 
00482   /* negation */
00483 
00484 #define NEGATE_DFVEC3(a) ( (a).xyz[0] = -(a).xyz[0] , \
00485                           (a).xyz[1] = -(a).xyz[1] , \
00486                           (a).xyz[2] = -(a).xyz[2]    )
00487 
00488   /* subtraction */
00489 
00490 #define SUB_DFVEC3(a,b) \
00491    ( tempA_dfvec3.xyz[0] = (a).xyz[0] - (b).xyz[0] , \
00492      tempA_dfvec3.xyz[1] = (a).xyz[1] - (b).xyz[1] , \
00493      tempA_dfvec3.xyz[2] = (a).xyz[2] - (b).xyz[2] , tempA_dfvec3 )
00494 
00495   /* addition */
00496 
00497 #define ADD_DFVEC3(a,b) \
00498    ( tempA_dfvec3.xyz[0] = (a).xyz[0] + (b).xyz[0] , \
00499      tempA_dfvec3.xyz[1] = (a).xyz[1] + (b).xyz[1] , \
00500      tempA_dfvec3.xyz[2] = (a).xyz[2] + (b).xyz[2] , tempA_dfvec3 )
00501 
00502   /* make into a unit vector */
00503 
00504 #define NORMALIZE_DFVEC3(a) \
00505    ( dtempRWC =   (a).xyz[0] * (a).xyz[0]                 \
00506                + (a).xyz[1] * (a).xyz[1]                 \
00507                + (a).xyz[2] * (a).xyz[2]               , \
00508      dtempRWC = (dtempRWC > 0) ? (1.0/sqrt(dtempRWC)) : 0 , \
00509      tempA_dfvec3.xyz[0] = (a).xyz[0] * dtempRWC         , \
00510      tempA_dfvec3.xyz[1] = (a).xyz[1] * dtempRWC         , \
00511      tempA_dfvec3.xyz[2] = (a).xyz[2] * dtempRWC         , tempA_dfvec3 )
00512 
00513   /* cross product */
00514 
00515 #define CROSS_DFVEC3(a,b) \
00516   ( tempA_dfvec3.xyz[0] = (a).xyz[1]*(b).xyz[2] - (a).xyz[2]*(b).xyz[1] , \
00517     tempA_dfvec3.xyz[1] = (a).xyz[2]*(b).xyz[0] - (a).xyz[0]*(b).xyz[2] , \
00518     tempA_dfvec3.xyz[2] = (a).xyz[0]*(b).xyz[1] - (a).xyz[1]*(b).xyz[0] , \
00519     tempA_dfvec3 )
00520 
00521   /* L2 norm */
00522 
00523 #define SIZE_DFVEC3(a) \
00524    sqrt((a).xyz[0]*(a).xyz[0]+(a).xyz[1]*(a).xyz[1]+(a).xyz[2]*(a).xyz[2])
00525 
00526   /* dot product */
00527 
00528 #define DOT_DFVEC3(a,b) \
00529    ((a).xyz[0]*(b).xyz[0] + (a).xyz[1]*(b).xyz[1] + (a).xyz[2]*(b).xyz[2])
00530 
00531   /* scale and add two vectors: fa * a + fb * b */
00532 
00533 #define SCLADD_DFVEC3(fa,a,fb,b) \
00534   ( tempA_dfvec3.xyz[0] = (fa)*(a).xyz[0] + (fb)*(b).xyz[0] , \
00535     tempA_dfvec3.xyz[1] = (fa)*(a).xyz[1] + (fb)*(b).xyz[1] , \
00536     tempA_dfvec3.xyz[2] = (fa)*(a).xyz[2] + (fb)*(b).xyz[2] , tempA_dfvec3 )
00537 
00538   /* round to an integer vector (assume non-negative) */
00539 
00540 #define INT_DFVEC3(a)                      \
00541   ( tempA_ivec3.ijk[0] = (a).xyz[0] + 0.5 , \
00542     tempA_ivec3.ijk[1] = (a).xyz[1] + 0.5 ,  \
00543     tempA_ivec3.ijk[1] = (a).xyz[2] + 0.5 , tempA_ivec3 ) ;
00544 
00545 #define SIZE_DMAT(A)                                                \
00546   ( fabs((A).mat[0][0]) + fabs((A).mat[0][1]) + fabs((A).mat[0][2])  \
00547    +fabs((A).mat[1][0]) + fabs((A).mat[1][1]) + fabs((A).mat[1][2])   \
00548    +fabs((A).mat[2][0]) + fabs((A).mat[2][1]) + fabs((A).mat[2][2]) )
00549 
00550   /* matrix-vector multiply */
00551 
00552 #define DMATVEC(A,x) \
00553   ( tempA_dfvec3.xyz[0] = (A).mat[0][0] * (x).xyz[0]  \
00554                         +(A).mat[0][1] * (x).xyz[1]  \
00555                         +(A).mat[0][2] * (x).xyz[2] ,\
00556     tempA_dfvec3.xyz[1] = (A).mat[1][0] * (x).xyz[0]  \
00557                         +(A).mat[1][1] * (x).xyz[1]  \
00558                         +(A).mat[1][2] * (x).xyz[2] ,\
00559     tempA_dfvec3.xyz[2] = (A).mat[2][0] * (x).xyz[0]  \
00560                         +(A).mat[2][1] * (x).xyz[1]  \
00561                         +(A).mat[2][2] * (x).xyz[2] ,  tempA_dfvec3 )
00562 
00563   /* matrix-vector multiply with subtract: output = A (x-b) */
00564 
00565 #define VECSUB_DMAT(A,x,b) \
00566   ( tempA_dfvec3.xyz[0] = (A).mat[0][0] * ((x).xyz[0]-(b).xyz[0])  \
00567                          +(A).mat[0][1] * ((x).xyz[1]-(b).xyz[1])  \
00568                          +(A).mat[0][2] * ((x).xyz[2]-(b).xyz[2]) ,\
00569     tempA_dfvec3.xyz[1] = (A).mat[1][0] * ((x).xyz[0]-(b).xyz[0])  \
00570                          +(A).mat[1][1] * ((x).xyz[1]-(b).xyz[1])  \
00571                          +(A).mat[1][2] * ((x).xyz[2]-(b).xyz[2]) ,\
00572     tempA_dfvec3.xyz[2] = (A).mat[2][0] * ((x).xyz[0]-(b).xyz[0])  \
00573                          +(A).mat[2][1] * ((x).xyz[1]-(b).xyz[1])  \
00574                          +(A).mat[2][2] * ((x).xyz[2]-(b).xyz[2]) ,\
00575     tempA_dfvec3 )
00576 
00577    /* matrix vector multiply with subtract after: A x - b */
00578 
00579 #define DMATVEC_SUB(A,x,b) \
00580   ( tempA_dfvec3.xyz[0] = (A).mat[0][0] * (x).xyz[0]               \
00581                          +(A).mat[0][1] * (x).xyz[1]               \
00582                          +(A).mat[0][2] * (x).xyz[2] - (b).xyz[0] ,\
00583     tempA_dfvec3.xyz[1] = (A).mat[1][0] * (x).xyz[0]               \
00584                          +(A).mat[1][1] * (x).xyz[1]               \
00585                          +(A).mat[1][2] * (x).xyz[2] - (b).xyz[1] ,\
00586     tempA_dfvec3.xyz[2] = (A).mat[2][0] * (x).xyz[0]               \
00587                          +(A).mat[2][1] * (x).xyz[1]               \
00588                          +(A).mat[2][2] * (x).xyz[2] - (b).xyz[2] ,\
00589     tempA_dfvec3 )
00590 
00591    /* matrix vector multiply with add after: A x + b */
00592 
00593 #define DMATVEC_ADD(A,x,b) \
00594   ( tempA_dfvec3.xyz[0] = (A).mat[0][0] * (x).xyz[0]               \
00595                          +(A).mat[0][1] * (x).xyz[1]               \
00596                          +(A).mat[0][2] * (x).xyz[2] + (b).xyz[0] ,\
00597     tempA_dfvec3.xyz[1] = (A).mat[1][0] * (x).xyz[0]               \
00598                          +(A).mat[1][1] * (x).xyz[1]               \
00599                          +(A).mat[1][2] * (x).xyz[2] + (b).xyz[1] ,\
00600     tempA_dfvec3.xyz[2] = (A).mat[2][0] * (x).xyz[0]               \
00601                          +(A).mat[2][1] * (x).xyz[1]               \
00602                          +(A).mat[2][2] * (x).xyz[2] + (b).xyz[2] ,\
00603     tempA_dfvec3 )
00604 
00605   /* matrix-matrix multiply */
00606 
00607 #define ROW_DOT_COL(A,B,i,j) (  (A).mat[i][0] * (B).mat[0][j] \
00608                               + (A).mat[i][1] * (B).mat[1][j] \
00609                               + (A).mat[i][2] * (B).mat[2][j]   )
00610 
00611 #define DMAT_MUL(A,B) \
00612   ( tempA_dmat33.mat[0][0] = ROW_DOT_COL((A),(B),0,0) , \
00613     tempA_dmat33.mat[1][0] = ROW_DOT_COL((A),(B),1,0) , \
00614     tempA_dmat33.mat[2][0] = ROW_DOT_COL((A),(B),2,0) , \
00615     tempA_dmat33.mat[0][1] = ROW_DOT_COL((A),(B),0,1) , \
00616     tempA_dmat33.mat[1][1] = ROW_DOT_COL((A),(B),1,1) , \
00617     tempA_dmat33.mat[2][1] = ROW_DOT_COL((A),(B),2,1) , \
00618     tempA_dmat33.mat[0][2] = ROW_DOT_COL((A),(B),0,2) , \
00619     tempA_dmat33.mat[1][2] = ROW_DOT_COL((A),(B),1,2) , \
00620     tempA_dmat33.mat[2][2] = ROW_DOT_COL((A),(B),2,2) , tempA_dmat33 )
00621 
00622 #define DMAT_SCALAR(A,c)                          \
00623   ( tempA_dmat33.mat[0][0] = (c)*(A).mat[0][0] ,  \
00624     tempA_dmat33.mat[1][0] = (c)*(A).mat[1][0] ,  \
00625     tempA_dmat33.mat[2][0] = (c)*(A).mat[2][0] ,  \
00626     tempA_dmat33.mat[0][1] = (c)*(A).mat[0][1] ,  \
00627     tempA_dmat33.mat[1][1] = (c)*(A).mat[1][1] ,  \
00628     tempA_dmat33.mat[2][1] = (c)*(A).mat[2][1] ,  \
00629     tempA_dmat33.mat[0][2] = (c)*(A).mat[0][2] ,  \
00630     tempA_dmat33.mat[1][2] = (c)*(A).mat[1][2] ,  \
00631     tempA_dmat33.mat[2][2] = (c)*(A).mat[2][2] , tempA_dmat33 )
00632 
00633    /* matrix determinant */
00634 
00635 #define DMAT_DET(A) \
00636  (  (A).mat[0][0]*(A).mat[1][1]*(A).mat[2][2] \
00637   - (A).mat[0][0]*(A).mat[1][2]*(A).mat[2][1] \
00638   - (A).mat[1][0]*(A).mat[0][1]*(A).mat[2][2] \
00639   + (A).mat[1][0]*(A).mat[0][2]*(A).mat[2][1] \
00640   + (A).mat[2][0]*(A).mat[0][1]*(A).mat[1][2] \
00641   - (A).mat[2][0]*(A).mat[0][2]*(A).mat[1][1]   )
00642 
00643    /* matrix trace [5 Oct 1998] */
00644 
00645 #define DMAT_TRACE(A) ( (A).mat[0][0] + (A).mat[1][1] + (A).mat[2][2] )
00646 
00647    /* matrix inverse */
00648 
00649 #define DMAT_INV(A) \
00650  ( dtempRWC = 1.0 / DMAT_DET(A) , \
00651     tempA_dmat33.mat[1][1] = \
00652      ( (A).mat[0][0]*(A).mat[2][2] - (A).mat[0][2]*(A).mat[2][0]) * dtempRWC,\
00653     tempA_dmat33.mat[2][2] = \
00654      ( (A).mat[0][0]*(A).mat[1][1] - (A).mat[0][1]*(A).mat[1][0]) * dtempRWC,\
00655     tempA_dmat33.mat[2][0] = \
00656      ( (A).mat[1][0]*(A).mat[2][1] - (A).mat[1][1]*(A).mat[2][0]) * dtempRWC,\
00657     tempA_dmat33.mat[1][2] = \
00658      (-(A).mat[0][0]*(A).mat[1][2] + (A).mat[0][2]*(A).mat[1][0]) * dtempRWC,\
00659     tempA_dmat33.mat[0][1] = \
00660      (-(A).mat[0][1]*(A).mat[2][2] + (A).mat[0][2]*(A).mat[2][1]) * dtempRWC,\
00661     tempA_dmat33.mat[0][0] = \
00662      ( (A).mat[1][1]*(A).mat[2][2] - (A).mat[1][2]*(A).mat[2][1]) * dtempRWC,\
00663     tempA_dmat33.mat[2][1] = \
00664      (-(A).mat[0][0]*(A).mat[2][1] + (A).mat[0][1]*(A).mat[2][0]) * dtempRWC,\
00665     tempA_dmat33.mat[1][0] = \
00666      (-(A).mat[1][0]*(A).mat[2][2] + (A).mat[1][2]*(A).mat[2][0]) * dtempRWC,\
00667     tempA_dmat33.mat[0][2] = \
00668      ( (A).mat[0][1]*(A).mat[1][2] - (A).mat[0][2]*(A).mat[1][1]) * dtempRWC,\
00669     tempA_dmat33 )
00670 
00671   /* load a matrix from scalars [3 Oct 1998] */
00672 
00673 #define LOAD_DMAT(A,a11,a12,a13,a21,a22,a23,a31,a32,a33) \
00674  ( (A).mat[0][0] = (a11) , (A).mat[0][1] = (a12) ,      \
00675    (A).mat[0][2] = (a13) , (A).mat[1][0] = (a21) ,      \
00676    (A).mat[1][1] = (a22) , (A).mat[1][2] = (a23) ,      \
00677    (A).mat[2][0] = (a31) , (A).mat[2][1] = (a32) , (A).mat[2][2] = (a33) )
00678 
00679   /* unload a matrix into scalars [3 Oct 1998] */
00680 
00681 #define UNLOAD_DMAT(A,a11,a12,a13,a21,a22,a23,a31,a32,a33) \
00682  ( (a11) = (A).mat[0][0] , (a12) = (A).mat[0][1] ,        \
00683    (a13) = (A).mat[0][2] , (a21) = (A).mat[1][0] ,        \
00684    (a22) = (A).mat[1][1] , (a23) = (A).mat[1][2] ,        \
00685    (a31) = (A).mat[2][0] , (a32) = (A).mat[2][1] , (a33) = (A).mat[2][2] )
00686 
00687    /* diagonal matrix */
00688 
00689 #define LOAD_DIAG_DMAT(A,x,y,z) \
00690  ( (A).mat[0][0] = (x) , \
00691    (A).mat[1][1] = (y) , \
00692    (A).mat[2][2] = (z) , \
00693    (A).mat[0][1] = (A).mat[0][2] = (A).mat[1][0] = \
00694    (A).mat[1][2] = (A).mat[2][0] = (A).mat[2][1] = 0.0 )
00695 
00696    /* zero matrix */
00697 
00698 #define LOAD_ZERO_DMAT(A) LOAD_DIAG_DMAT((A),0,0,0)
00699 
00700    /* elementary rotation matrices:
00701       rotate about axis #ff, from axis #aa toward #bb,
00702       where ff, aa, and bb are a permutation of {0,1,2} */
00703 
00704 #define LOAD_ROTGEN_DMAT(A,th,ff,aa,bb)             \
00705  ( (A).mat[aa][aa] = (A).mat[bb][bb] = cos((th)) , \
00706    (A).mat[aa][bb] = sin((th)) ,                   \
00707    (A).mat[bb][aa] = -(A).mat[aa][bb] ,            \
00708    (A).mat[ff][ff] = 1.0 ,                         \
00709    (A).mat[aa][ff] = (A).mat[bb][ff] = (A).mat[ff][aa] = (A).mat[ff][bb] = 0.0 )
00710 
00711 #define LOAD_ROTX_DMAT(A,th) LOAD_ROTGEN_DMAT(A,th,0,1,2)
00712 #define LOAD_ROTY_DMAT(A,th) LOAD_ROTGEN_DMAT(A,th,1,2,0)
00713 #define LOAD_ROTZ_DMAT(A,th) LOAD_ROTGEN_DMAT(A,th,2,0,1)
00714 
00715 #define LOAD_ROT_DMAT(A,th,i)                  \
00716   do{ switch( (i) ){                          \
00717         case 0: LOAD_ROTX_DMAT(A,th) ; break ; \
00718         case 1: LOAD_ROTY_DMAT(A,th) ; break ; \
00719         case 2: LOAD_ROTZ_DMAT(A,th) ; break ; \
00720        default: LOAD_ZERO_DMAT(A)    ; break ; \
00721       } } while(0)
00722 
00723    /* shear matrices [3 Oct 1998] */
00724 
00725 #define LOAD_SHEARX_DMAT(A,f,b,c) ( LOAD_DIAG_DMAT(A,(f),1,1) , \
00726                                    (A).mat[0][1] = (b) , (A).mat[0][2] = (c) )
00727 
00728 #define LOAD_SHEARY_DMAT(A,f,a,c) ( LOAD_DIAG_DMAT(A,1,(f),1) , \
00729                                    (A).mat[1][0] = (a) , (A).mat[1][2] = (c) )
00730 
00731 #define LOAD_SHEARZ_DMAT(A,f,a,b) ( LOAD_DIAG_DMAT(A,1,1,(f)) , \
00732                                    (A).mat[2][0] = (a) , (A).mat[2][1] = (b) )
00733 
00734    /* matrix transpose */
00735 
00736 #define TRANSPOSE_DMAT(A)                   \
00737  ( tempA_dmat33.mat[0][0] = (A).mat[0][0] , \
00738    tempA_dmat33.mat[1][0] = (A).mat[0][1] , \
00739    tempA_dmat33.mat[2][0] = (A).mat[0][2] , \
00740    tempA_dmat33.mat[0][1] = (A).mat[1][0] , \
00741    tempA_dmat33.mat[1][1] = (A).mat[1][1] , \
00742    tempA_dmat33.mat[2][1] = (A).mat[1][2] , \
00743    tempA_dmat33.mat[0][2] = (A).mat[2][0] , \
00744    tempA_dmat33.mat[1][2] = (A).mat[2][1] , \
00745    tempA_dmat33.mat[2][2] = (A).mat[2][2] , tempA_dmat33 )
00746 
00747    /* matrix add & subtract */
00748 
00749 #define ADD_DMAT(A,B)                                       \
00750  ( tempA_dmat33.mat[0][0] = (A).mat[0][0] + (B).mat[0][0] , \
00751    tempA_dmat33.mat[1][0] = (A).mat[1][0] + (B).mat[1][0] , \
00752    tempA_dmat33.mat[2][0] = (A).mat[2][0] + (B).mat[2][0] , \
00753    tempA_dmat33.mat[0][1] = (A).mat[0][1] + (B).mat[0][1] , \
00754    tempA_dmat33.mat[1][1] = (A).mat[1][1] + (B).mat[1][1] , \
00755    tempA_dmat33.mat[2][1] = (A).mat[2][1] + (B).mat[2][1] , \
00756    tempA_dmat33.mat[0][2] = (A).mat[0][2] + (B).mat[0][2] , \
00757    tempA_dmat33.mat[1][2] = (A).mat[1][2] + (B).mat[1][2] , \
00758    tempA_dmat33.mat[2][2] = (A).mat[2][2] + (B).mat[2][2] , tempA_dmat33 )
00759 
00760 #define SUB_DMAT(A,B)                                       \
00761  ( tempA_dmat33.mat[0][0] = (A).mat[0][0] - (B).mat[0][0] , \
00762    tempA_dmat33.mat[1][0] = (A).mat[1][0] - (B).mat[1][0] , \
00763    tempA_dmat33.mat[2][0] = (A).mat[2][0] - (B).mat[2][0] , \
00764    tempA_dmat33.mat[0][1] = (A).mat[0][1] - (B).mat[0][1] , \
00765    tempA_dmat33.mat[1][1] = (A).mat[1][1] - (B).mat[1][1] , \
00766    tempA_dmat33.mat[2][1] = (A).mat[2][1] - (B).mat[2][1] , \
00767    tempA_dmat33.mat[0][2] = (A).mat[0][2] - (B).mat[0][2] , \
00768    tempA_dmat33.mat[1][2] = (A).mat[1][2] - (B).mat[1][2] , \
00769    tempA_dmat33.mat[2][2] = (A).mat[2][2] - (B).mat[2][2] , tempA_dmat33 )
00770 
00771    /* component-wise min and max of 3-vectors */
00772 
00773 #define MAX_DFVEC3(a,b) \
00774 ( tempA_dfvec3.xyz[0] = (((a).xyz[0] > (b).xyz[0]) ? (a).xyz[0] : (b).xyz[0]) ,\
00775   tempA_dfvec3.xyz[1] = (((a).xyz[1] > (b).xyz[1]) ? (a).xyz[1] : (b).xyz[1]) ,\
00776   tempA_dfvec3.xyz[2] = (((a).xyz[2] > (b).xyz[2]) ? (a).xyz[2] : (b).xyz[2]) ,\
00777   tempA_dfvec3 )
00778 
00779 #define MIN_DFVEC3(a,b) \
00780 ( tempA_dfvec3.xyz[0] = (((a).xyz[0] < (b).xyz[0]) ? (a).xyz[0] : (b).xyz[0]) ,\
00781   tempA_dfvec3.xyz[1] = (((a).xyz[1] < (b).xyz[1]) ? (a).xyz[1] : (b).xyz[1]) ,\
00782   tempA_dfvec3.xyz[2] = (((a).xyz[2] < (b).xyz[2]) ? (a).xyz[2] : (b).xyz[2]) ,\
00783   tempA_dfvec3 )
00784 
00785    /* multiply two vecmat structures */
00786 
00787 static THD_dvecmat tempA_dvm33 ;
00788 
00789 #define MUL_DVECMAT(A,B)                                             \
00790   ( tempA_dvm33.mm = DMAT_MUL((A).mm,(B).mm) ,                       \
00791     tempB_dfvec3   = DMATVEC((A).mm,(B).vv) ,                        \
00792     tempA_dvm33.vv = ADD_DFVEC3(tempB_dfvec3,(A).vv) , tempA_dvm33 )
00793 
00794    /* invert a vecmat structure:                         */
00795    /* [y] = [R][x] + [v]       is transformation, so     */
00796    /* [x] = inv[R] - inv[R][v] is inverse transformation */
00797 
00798 #define INV_DVECMAT(A) ( tempA_dvm33.mm = DMAT_INV((A).mm) ,                \
00799                          tempA_dvm33.vv = DMATVEC(tempA_dvm33.mm,(A).vv) ,  \
00800                          NEGATE_DFVEC3(tempA_dvm33.vv) , tempA_dvm33       )
00801 
00802     /* same for single precision stuctures */
00803 
00804 static THD_vecmat  tempA_vm33 ;
00805 
00806 #define MUL_VECMAT(A,B)                                          \
00807   ( tempA_vm33.mm = MAT_MUL((A).mm,(B).mm) ,                     \
00808     tempB_fvec3   = MATVEC((A).mm,(B).vv) ,                      \
00809     tempA_vm33.vv = ADD_FVEC3(tempB_fvec3,(A).vv) , tempA_vm33 )
00810 
00811 #define INV_VECMAT(A) ( tempA_vm33.mm = MAT_INV((A).mm) ,                \
00812                         tempA_vm33.vv = MATVEC(tempA_vm33.mm,(A).vv) ,   \
00813                         NEGATE_FVEC3(tempA_vm33.vv) , tempA_vm33       )
00814 
00815     /* apply a vecmat to a vector */
00816 
00817 #define VECMAT_VEC(A,x)   MATVEC_ADD( (A).mm , (x) , (A).vv )
00818 #define DVECMAT_VEC(A,x) DMATVEC_ADD( (A).mm , (x) , (A).vv )
00819 
00820 #endif /* _MCW_3DVECMAT_ */
AFNI/NIfTI Server

Sections

Personal tools

Navigation

Doxygen Source Code Documentation

vecmat.h
