| 1 | #include "slalib.h"
|
|---|
| 2 | #include "slamac.h"
|
|---|
| 3 | void slaCc62s ( float v[6],
|
|---|
| 4 | float *a, float *b, float *r,
|
|---|
| 5 | float *ad, float *bd, float *rd )
|
|---|
| 6 | /*
|
|---|
| 7 | ** - - - - - - - - -
|
|---|
| 8 | ** s l a C c 6 2 s
|
|---|
| 9 | ** - - - - - - - - -
|
|---|
| 10 | **
|
|---|
| 11 | ** Conversion of position & velocity in Cartesian coordinates
|
|---|
| 12 | ** to spherical coordinates.
|
|---|
| 13 | **
|
|---|
| 14 | ** (single precision)
|
|---|
| 15 | **
|
|---|
| 16 | ** Given:
|
|---|
| 17 | ** v float[6] Cartesian position & velocity vector
|
|---|
| 18 | **
|
|---|
| 19 | ** Returned:
|
|---|
| 20 | ** *a float longitude (radians)
|
|---|
| 21 | ** *b float latitude (radians)
|
|---|
| 22 | ** *r float radial coordinate
|
|---|
| 23 | ** *ad float longitude derivative (radians per unit time)
|
|---|
| 24 | ** *bd float latitude derivative (radians per unit time)
|
|---|
| 25 | ** *rd float radial derivative
|
|---|
| 26 | **
|
|---|
| 27 | ** Last revision: 28 April 1996
|
|---|
| 28 | **
|
|---|
| 29 | ** Copyright P.T.Wallace. All rights reserved.
|
|---|
| 30 | */
|
|---|
| 31 | {
|
|---|
| 32 | double x, y, z, xd, yd, zd, rxy2, rxy, r2, xyp, dr;
|
|---|
| 33 |
|
|---|
| 34 |
|
|---|
| 35 | /* Components of position/velocity vector. */
|
|---|
| 36 | x = v[0];
|
|---|
| 37 | y = v[1];
|
|---|
| 38 | z = v[2];
|
|---|
| 39 | xd = v[3];
|
|---|
| 40 | yd = v[4];
|
|---|
| 41 | zd = v[5];
|
|---|
| 42 |
|
|---|
| 43 | /* Component of R in XY plane squared. */
|
|---|
| 44 | rxy2 = x * x + y * y;
|
|---|
| 45 |
|
|---|
| 46 | /* Modulus squared, with protection against null vector. */
|
|---|
| 47 | if ( ( r2 = rxy2 + z * z ) == 0.0 ) {
|
|---|
| 48 | x = xd;
|
|---|
| 49 | y = yd;
|
|---|
| 50 | z = zd;
|
|---|
| 51 | rxy2 = x * x + y * y;
|
|---|
| 52 | r2 = rxy2 + z * z;
|
|---|
| 53 | }
|
|---|
| 54 |
|
|---|
| 55 | /* Position and velocity in spherical coordinates. */
|
|---|
| 56 | rxy = sqrt ( rxy2 );
|
|---|
| 57 | xyp = x * xd + y * yd;
|
|---|
| 58 | if ( rxy2 != 0.0 ) {
|
|---|
| 59 | *a = (float) atan2 ( y, x );
|
|---|
| 60 | *b = (float) atan2 ( z, rxy );
|
|---|
| 61 | *ad = (float) ( ( x * yd - y * xd ) / rxy2 );
|
|---|
| 62 | *bd = (float) ( ( zd * rxy2 - z * xyp ) / ( r2 * rxy ) );
|
|---|
| 63 | } else {
|
|---|
| 64 | *a = 0.0f;
|
|---|
| 65 | *b = (float) ( ( z != 0.0 ) ? atan2 ( z, rxy ) : 0.0 );
|
|---|
| 66 | *ad = 0.0f;
|
|---|
| 67 | *bd = 0.0f;
|
|---|
| 68 | }
|
|---|
| 69 | *r = (float) ( dr = sqrt ( r2 ) );
|
|---|
| 70 | *rd = (float) ( ( dr != 0.0 ) ? ( xyp + z * zd ) / dr : 0.0 );
|
|---|
| 71 | }
|
|---|
