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