| 1 | #include "slalib.h"
|
|---|
| 2 | #include "slamac.h"
|
|---|
| 3 | #include <string.h>
|
|---|
| 4 | void slaDeuler ( char *order, double phi, double theta,
|
|---|
| 5 | double psi, double rmat[3][3] )
|
|---|
| 6 | /*
|
|---|
| 7 | ** - - - - - - - - - -
|
|---|
| 8 | ** s l a D e u l e r
|
|---|
| 9 | ** - - - - - - - - - -
|
|---|
| 10 | **
|
|---|
| 11 | ** Form a rotation matrix from the Euler angles - three successive
|
|---|
| 12 | ** rotations about specified Cartesian axes.
|
|---|
| 13 | **
|
|---|
| 14 | ** (double precision)
|
|---|
| 15 | **
|
|---|
| 16 | ** Given:
|
|---|
| 17 | ** *order char specifies about which axes the rotations occur
|
|---|
| 18 | ** phi double 1st rotation (radians)
|
|---|
| 19 | ** theta double 2nd rotation ( " )
|
|---|
| 20 | ** psi double 3rd rotation ( " )
|
|---|
| 21 | **
|
|---|
| 22 | ** Returned:
|
|---|
| 23 | ** rmat double[3][3] rotation matrix
|
|---|
| 24 | **
|
|---|
| 25 | ** A rotation is positive when the reference frame rotates
|
|---|
| 26 | ** anticlockwise as seen looking towards the origin from the
|
|---|
| 27 | ** positive region of the specified axis.
|
|---|
| 28 | **
|
|---|
| 29 | ** The characters of order define which axes the three successive
|
|---|
| 30 | ** rotations are about. A typical value is 'zxz', indicating that
|
|---|
| 31 | ** rmat is to become the direction cosine matrix corresponding to
|
|---|
| 32 | ** rotations of the reference frame through phi radians about the
|
|---|
| 33 | ** old z-axis, followed by theta radians about the resulting x-axis,
|
|---|
| 34 | ** then psi radians about the resulting z-axis.
|
|---|
| 35 | **
|
|---|
| 36 | ** The axis names can be any of the following, in any order or
|
|---|
| 37 | ** combination: x, y, z, uppercase or lowercase, 1, 2, 3. Normal
|
|---|
| 38 | ** axis labelling/numbering conventions apply; the xyz (=123)
|
|---|
| 39 | ** triad is right-handed. Thus, the 'zxz' example given above
|
|---|
| 40 | ** could be written 'zxz' or '313' (or even 'zxz' or '3xz'). Order
|
|---|
| 41 | ** is terminated by length or by the first unrecognized character.
|
|---|
| 42 | **
|
|---|
| 43 | ** Fewer than three rotations are acceptable, in which case the later
|
|---|
| 44 | ** angle arguments are ignored. Zero rotations leaves rmat set to the
|
|---|
| 45 | ** identity matrix.
|
|---|
| 46 | **
|
|---|
| 47 | ** Last revision: 9 December 1996
|
|---|
| 48 | **
|
|---|
| 49 | ** Copyright P.T.Wallace. All rights reserved.
|
|---|
| 50 | */
|
|---|
| 51 | {
|
|---|
| 52 | int j, i, l, n, k;
|
|---|
| 53 | double result[3][3], rotn[3][3], angle, s, c , w, wm[3][3];
|
|---|
| 54 | char axis;
|
|---|
| 55 |
|
|---|
| 56 | /* Initialize result matrix */
|
|---|
| 57 | for ( j = 0; j < 3; j++ ) {
|
|---|
| 58 | for ( i = 0; i < 3; i++ ) {
|
|---|
| 59 | result[i][j] = ( i == j ) ? 1.0 : 0.0;
|
|---|
| 60 | }
|
|---|
| 61 | }
|
|---|
| 62 |
|
|---|
| 63 | /* Establish length of axis string */
|
|---|
| 64 | l = strlen ( order );
|
|---|
| 65 |
|
|---|
| 66 | /* Look at each character of axis string until finished */
|
|---|
| 67 | for ( n = 0; n < 3; n++ ) {
|
|---|
| 68 | if ( n <= l ) {
|
|---|
| 69 |
|
|---|
| 70 | /* Initialize rotation matrix for the current rotation */
|
|---|
| 71 | for ( j = 0; j < 3; j++ ) {
|
|---|
| 72 | for ( i = 0; i < 3; i++ ) {
|
|---|
| 73 | rotn[i][j] = ( i == j ) ? 1.0 : 0.0;
|
|---|
| 74 | }
|
|---|
| 75 | }
|
|---|
| 76 |
|
|---|
| 77 | /* Pick up the appropriate Euler angle and take sine & cosine */
|
|---|
| 78 | switch ( n ) {
|
|---|
| 79 | case 0 :
|
|---|
| 80 | angle = phi;
|
|---|
| 81 | break;
|
|---|
| 82 | case 1 :
|
|---|
| 83 | angle = theta;
|
|---|
| 84 | break;
|
|---|
| 85 | default:
|
|---|
| 86 | angle = psi;
|
|---|
| 87 | break;
|
|---|
| 88 | }
|
|---|
| 89 | s = sin ( angle );
|
|---|
| 90 | c = cos ( angle );
|
|---|
| 91 |
|
|---|
| 92 | /* Identify the axis */
|
|---|
| 93 | axis = order[n];
|
|---|
| 94 | if ( ( axis == 'X' ) || ( axis == 'x' ) || ( axis == '1' ) ) {
|
|---|
| 95 |
|
|---|
| 96 | /* Matrix for x-rotation */
|
|---|
| 97 | rotn[1][1] = c;
|
|---|
| 98 | rotn[1][2] = s;
|
|---|
| 99 | rotn[2][1] = -s;
|
|---|
| 100 | rotn[2][2] = c;
|
|---|
| 101 | }
|
|---|
| 102 | else if ( ( axis == 'Y' ) || ( axis == 'y' ) || ( axis == '2' ) ) {
|
|---|
| 103 |
|
|---|
| 104 | /* Matrix for y-rotation */
|
|---|
| 105 | rotn[0][0] = c;
|
|---|
| 106 | rotn[0][2] = -s;
|
|---|
| 107 | rotn[2][0] = s;
|
|---|
| 108 | rotn[2][2] = c;
|
|---|
| 109 | }
|
|---|
| 110 | else if ( ( axis == 'Z' ) || ( axis == 'z' ) || ( axis == '3' ) ) {
|
|---|
| 111 |
|
|---|
| 112 | /* Matrix for z-rotation */
|
|---|
| 113 | rotn[0][0] = c;
|
|---|
| 114 | rotn[0][1] = s;
|
|---|
| 115 | rotn[1][0] = -s;
|
|---|
| 116 | rotn[1][1] = c;
|
|---|
| 117 | } else {
|
|---|
| 118 |
|
|---|
| 119 | /* Unrecognized character - fake end of string */
|
|---|
| 120 | l = 0;
|
|---|
| 121 | }
|
|---|
| 122 |
|
|---|
| 123 | /* Apply the current rotation (matrix rotn x matrix result) */
|
|---|
| 124 | for ( i = 0; i < 3; i++ ) {
|
|---|
| 125 | for ( j = 0; j < 3; j++ ) {
|
|---|
| 126 | w = 0.0;
|
|---|
| 127 | for ( k = 0; k < 3; k++ ) {
|
|---|
| 128 | w += rotn[i][k] * result[k][j];
|
|---|
| 129 | }
|
|---|
| 130 | wm[i][j] = w;
|
|---|
| 131 | }
|
|---|
| 132 | }
|
|---|
| 133 | for ( j = 0; j < 3; j++ ) {
|
|---|
| 134 | for ( i= 0; i < 3; i++ ) {
|
|---|
| 135 | result[i][j] = wm[i][j];
|
|---|
| 136 | }
|
|---|
| 137 | }
|
|---|
| 138 | }
|
|---|
| 139 | }
|
|---|
| 140 |
|
|---|
| 141 | /* Copy the result */
|
|---|
| 142 | for ( j = 0; j < 3; j++ ) {
|
|---|
| 143 | for ( i = 0; i < 3; i++ ) {
|
|---|
| 144 | rmat[i][j] = result[i][j];
|
|---|
| 145 | }
|
|---|
| 146 | }
|
|---|
| 147 | }
|
|---|
