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 }
|
