Doxygen Source Code Documentation
rotmat.c File Reference
Go to the source code of this file.
Defines | |
#define | V2PI 6.2831853071795864 |
#define | VPI 3.1415926535897932 |
#define | VHPI 1.5707963267948966 |
#define | VEPS 0.00001 |
Functions | |
THD_dmat33 | RCREND_axis_rotmatrix (double th, double a, double b, double c) |
Define Documentation
|
Definition at line 1 of file rotmat.c. Referenced by RCREND_axis_rotmatrix(). |
|
Definition at line 4 of file rotmat.c. Referenced by RCREND_axis_rotmatrix(). |
|
Definition at line 3 of file rotmat.c. Referenced by RCREND_axis_rotmatrix(). |
|
Definition at line 2 of file rotmat.c. Referenced by RCREND_axis_rotmatrix(). |
Function Documentation
|
Compute rotation matrix about axis (a,b,c) of angle th radians. Definition at line 8 of file rotmat.c. References a, ADD_DMAT, c, DMAT_INV, DMAT_MUL, LOAD_DIAG_DMAT, LOAD_DMAT, THD_dmat33::mat, nc, SUB_DMAT, V2PI, VEPS, VHPI, and VPI.
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 } |