| 1 | #include "slalib.h"
|
|---|
| 2 | #include "slamac.h"
|
|---|
| 3 | void slaAv2m ( float axvec[3], float rmat[3][3] )
|
|---|
| 4 | /*
|
|---|
| 5 | ** - - - - - - - -
|
|---|
| 6 | ** s l a A v 2 m
|
|---|
| 7 | ** - - - - - - - -
|
|---|
| 8 | **
|
|---|
| 9 | ** Form the rotation matrix corresponding to a given axial vector.
|
|---|
| 10 | **
|
|---|
| 11 | ** (single precision)
|
|---|
| 12 | **
|
|---|
| 13 | ** A rotation matrix describes a rotation about some arbitrary axis.
|
|---|
| 14 | ** The axis is called the Euler axis, and the angle through which the
|
|---|
| 15 | ** reference frame rotates is called the Euler angle. The axial
|
|---|
| 16 | ** vector supplied to this routine has the same direction as the
|
|---|
| 17 | ** Euler axis, and its magnitude is the Euler angle in radians.
|
|---|
| 18 | **
|
|---|
| 19 | ** Given:
|
|---|
| 20 | ** axvec float[3] axial vector (radians)
|
|---|
| 21 | **
|
|---|
| 22 | ** Returned:
|
|---|
| 23 | ** rmat float[3][3] rotation matrix
|
|---|
| 24 | **
|
|---|
| 25 | ** If axvec is null, the unit matrix is returned.
|
|---|
| 26 | **
|
|---|
| 27 | ** The reference frame rotates clockwise as seen looking along
|
|---|
| 28 | ** the axial vector from the origin.
|
|---|
| 29 | **
|
|---|
| 30 | ** Last revision: 25 July 1993
|
|---|
| 31 | **
|
|---|
| 32 | ** Copyright P.T.Wallace. All rights reserved.
|
|---|
| 33 | */
|
|---|
| 34 | {
|
|---|
| 35 | double x, y, z, phi, s, c, w;
|
|---|
| 36 |
|
|---|
| 37 | /* Euler angle - magnitude of axial vector - and functions */
|
|---|
| 38 | x = (double) axvec[0];
|
|---|
| 39 | y = (double) axvec[1];
|
|---|
| 40 | z = (double) axvec[2];
|
|---|
| 41 | phi = sqrt ( x * x + y * y + z * z );
|
|---|
| 42 | s = sin ( phi );
|
|---|
| 43 | c = cos ( phi );
|
|---|
| 44 | w = 1.0 - c;
|
|---|
| 45 |
|
|---|
| 46 | /* Euler axis - direction of axial vector (perhaps null) */
|
|---|
| 47 | if ( phi != 0.0 ) {
|
|---|
| 48 | x = x / phi;
|
|---|
| 49 | y = y / phi;
|
|---|
| 50 | z = z / phi;
|
|---|
| 51 | }
|
|---|
| 52 |
|
|---|
| 53 | /* Compute the rotation matrix */
|
|---|
| 54 | rmat[0][0] = (float) ( x * x * w + c );
|
|---|
| 55 | rmat[0][1] = (float) ( x * y * w + z * s );
|
|---|
| 56 | rmat[0][2] = (float) ( x * z * w - y * s );
|
|---|
| 57 | rmat[1][0] = (float) ( x * y * w - z * s );
|
|---|
| 58 | rmat[1][1] = (float) ( y * y * w + c );
|
|---|
| 59 | rmat[1][2] = (float) ( y * z * w + x * s );
|
|---|
| 60 | rmat[2][0] = (float) ( x * z * w + y * s );
|
|---|
| 61 | rmat[2][1] = (float) ( y * z * w - x * s );
|
|---|
| 62 | rmat[2][2] = (float) ( z * z * w + c );
|
|---|
| 63 | }
|
|---|
