38 #include <cpl_matrix.h>
40 #include "girvcorrection.h"
56 static const cxdouble dct0 = 2415020.0;
57 static const cxdouble dcjul = 36525.0;
58 static const cxdouble dc1900 = 1900.0;
59 static const cxdouble dctrop = 365.24219572;
60 static const cxdouble dcbes = 0.313;
62 static const cxdouble RV_DPI =
63 3.1415926535897932384626433832795028841971693993751;
65 static const cxdouble RV_D2PI =
66 6.2831853071795864769252867665590057683943387987502;
68 static const cxdouble RV_D4PI =
69 12.566370614359172953850573533118011536788677597500;
71 static const cxdouble RV_DPIBY2 =
72 1.5707963267948966192313216916397514420985846996876;
74 static const cxdouble RV_DD2R =
75 0.017453292519943295769236907684886127134428718885417;
77 static const cxdouble RV_DAS2R =
78 4.8481368110953599358991410235794797595635330237270e-6;
132 slaDeuler(
const cxchar* order, cxdouble phi, cxdouble theta, cxdouble psi,
135 register cxint j, i, l, n, k;
136 cxdouble result[3][3], rotn[3][3], angle, s, c , w, wm[3][3];
140 for ( j = 0; j < 3; j++ ) {
141 for ( i = 0; i < 3; i++ ) {
142 result[i][j] = ( i == j ) ? 1.0 : 0.0;
147 l = strlen ( order );
150 for ( n = 0; n < 3; n++ ) {
154 for ( j = 0; j < 3; j++ ) {
155 for ( i = 0; i < 3; i++ ) {
156 rotn[i][j] = ( i == j ) ? 1.0 : 0.0;
177 if ( ( axis ==
'X' ) || ( axis ==
'x' ) || ( axis ==
'1' ) ) {
185 else if ( ( axis ==
'Y' ) || ( axis ==
'y' ) || ( axis ==
'2' ) ) {
193 else if ( ( axis ==
'Z' ) || ( axis ==
'z' ) || ( axis ==
'3' ) ) {
208 for ( i = 0; i < 3; i++ ) {
209 for ( j = 0; j < 3; j++ ) {
211 for ( k = 0; k < 3; k++ ) {
212 w += rotn[i][k] * result[k][j];
217 for ( j = 0; j < 3; j++ ) {
218 for ( i= 0; i < 3; i++ ) {
219 result[i][j] = wm[i][j];
226 for ( j = 0; j < 3; j++ ) {
227 for ( i = 0; i < 3; i++ ) {
228 cpl_matrix_set(rmat, i, j, result[i][j]);
268 inline static cpl_matrix*
269 slaPrecession (cxdouble ep0, cxdouble ep1)
280 cpl_matrix* mprec = NULL;
287 t0 = ( ep0 - 2000.0 ) / 1000.0;
294 t = ( ep1 - ep0 ) / 1000.0;
301 tas2r = t * RV_DAS2R;
307 0.0007 * t0) * t0) * t0) * t0) * t0;
314 0.0007 * t0) * t0) * t0) * t0 +
318 0.0005 * t0) * t0) * t0 +
323 -0.0002 * t) * t) * t) * t) * t) * tas2r;
330 0.0026 * t0) * t0) * t0) * t0 +
334 0.0044 * t0) * t0) * t0 +
340 -0.0001 * t) * t) * t) * t) * t) * tas2r;
342 theta = (20042.0207 +
347 -0.0005 * t0) * t0) * t0) * t0) * t0 +
352 -0.0012 * t0) * t0) * t0) * t0 +
356 -0.0001 * t0) * t0) * t0 +
361 0.0011 * t0 + 0.0004 * t) * t) * t) * t) * t) * tas2r;
368 mprec = cpl_matrix_new(3, 3);
369 slaDeuler(
"ZYZ", -zeta, theta, -z, mprec);
392 inline static cxdouble
393 sideral_time(cxdouble djd, cxdouble dlong)
401 const cxdouble d1 = 1.739935934667999;
402 const cxdouble d2 = 6.283319509909095e02;
403 const cxdouble d3 = 6.755878646261384e-06;
405 const cxdouble df = 1.00273790934;
411 cxdouble djd0 = floor(djd) + 0.5;
417 dut = (djd - djd0) * RV_D2PI;
419 dt = (djd0 - dct0) / dcjul;
420 dst0 = d1 + d2 * dt + d3 * dt * dt;
421 dst0 = fmod(dst0, RV_D2PI);
422 dst = df * dut + dst0 - dlong;
423 dst = fmod(dst + RV_D4PI, RV_D2PI);
460 inline static cxdouble
461 geo_correction(cxdouble dlat, cxdouble dalt, cxdouble dec, cxdouble dha)
468 const cxdouble da = 6378.137;
474 const cxdouble df = 1./298.257222;
480 const cxdouble dw = RV_D2PI/86164.;
483 const cxdouble de2 = df * (2.0 - df);
484 const cxdouble dsdlats = sin (dlat) * sin (dlat);
498 d1 = 1.0 - de2 * (2.0 - de2) * dsdlats;
499 d2 = 1.0 - de2 * dsdlats;
500 dr0 = da * sqrt(d1 / d2);
507 d1 = de2 * sin(2.0 * dlat);
509 dlatg = dlat - atan(d1 / d2);
516 drh = dr0 * cos(dlatg) + (dalt / 1000.) * cos(dlat);
524 dvelg = dw * drh * cos(dec) * sin(dha);
566 earth_velocity(cxdouble dje, cxdouble deq, cxdouble*
const hvel,
567 cxdouble*
const bvel)
577 const cxdouble dcfel[][3] = {
578 {1.7400353e+00, 6.2833195099091e+02, 5.2796e-06},
579 {6.2565836e+00, 6.2830194572674e+02, -2.6180e-06},
580 {4.7199666e+00, 8.3997091449254e+03, -1.9780e-05},
581 {1.9636505e-01, 8.4334662911720e+03, -5.6044e-05},
582 {4.1547339e+00, 5.2993466764997e+01, 5.8845e-06},
583 {4.6524223e+00, 2.1354275911213e+01, 5.6797e-06},
584 {4.2620486e+00, 7.5025342197656e+00, 5.5317e-06},
585 {1.4740694e+00, 3.8377331909193e+00, 5.6093e-06}
595 const cxdouble dceps[3] = {
601 const cxdouble ccsel[][3] = {
602 {1.675104e-02, -4.179579e-05, -1.260516e-07},
603 {2.220221e-01, 2.809917e-02, 1.852532e-05},
604 {1.589963e+00, 3.418075e-02, 1.430200e-05},
605 {2.994089e+00, 2.590824e-02, 4.155840e-06},
606 {8.155457e-01, 2.486352e-02, 6.836840e-06},
607 {1.735614e+00, 1.763719e-02, 6.370440e-06},
608 {1.968564e+00, 1.524020e-02, -2.517152e-06},
609 {1.282417e+00, 8.703393e-03, 2.289292e-05},
610 {2.280820e+00, 1.918010e-02, 4.484520e-06},
611 {4.833473e-02, 1.641773e-04, -4.654200e-07},
612 {5.589232e-02, -3.455092e-04, -7.388560e-07},
613 {4.634443e-02, -2.658234e-05, 7.757000e-08},
614 {8.997041e-03, 6.329728e-06, -1.939256e-09},
615 {2.284178e-02, -9.941590e-05, 6.787400e-08},
616 {4.350267e-02, -6.839749e-05, -2.714956e-07},
617 {1.348204e-02, 1.091504e-05, 6.903760e-07},
618 {3.106570e-02, -1.665665e-04, -1.590188e-07}
629 const cxdouble dcargs[][2] = {
630 {5.0974222e+00, -7.8604195454652e+02},
631 {3.9584962e+00, -5.7533848094674e+02},
632 {1.6338070e+00, -1.1506769618935e+03},
633 {2.5487111e+00, -3.9302097727326e+02},
634 {4.9255514e+00, -5.8849265665348e+02},
635 {1.3363463e+00, -5.5076098609303e+02},
636 {1.6072053e+00, -5.2237501616674e+02},
637 {1.3629480e+00, -1.1790629318198e+03},
638 {5.5657014e+00, -1.0977134971135e+03},
639 {5.0708205e+00, -1.5774000881978e+02},
640 {3.9318944e+00, 5.2963464780000e+01},
641 {4.8989497e+00, 3.9809289073258e+01},
642 {1.3097446e+00, 7.7540959633708e+01},
643 {3.5147141e+00, 7.9618578146517e+01},
644 {3.5413158e+00, -5.4868336758022e+02}
654 const cxdouble ccamps[][5] = {
655 {-2.279594e-5, 1.407414e-5, 8.273188e-6, 1.340565e-5, -2.490817e-7},
656 {-3.494537e-5, 2.860401e-7, 1.289448e-7, 1.627237e-5, -1.823138e-7},
657 { 6.593466e-7, 1.322572e-5, 9.258695e-6, -4.674248e-7, -3.646275e-7},
658 { 1.140767e-5, -2.049792e-5, -4.747930e-6, -2.638763e-6, -1.245408e-7},
659 { 9.516893e-6, -2.748894e-6, -1.319381e-6, -4.549908e-6, -1.864821e-7},
660 { 7.310990e-6, -1.924710e-6, -8.772849e-7, -3.334143e-6, -1.745256e-7},
661 {-2.603449e-6, 7.359472e-6, 3.168357e-6, 1.119056e-6, -1.655307e-7},
662 {-3.228859e-6, 1.308997e-7, 1.013137e-7, 2.403899e-6, -3.736225e-7},
663 { 3.442177e-7, 2.671323e-6, 1.832858e-6, -2.394688e-7, -3.478444e-7},
664 { 8.702406e-6, -8.421214e-6, -1.372341e-6, -1.455234e-6, -4.998479e-8},
665 {-1.488378e-6, -1.251789e-5, 5.226868e-7, -2.049301e-7, 0.0e0},
666 {-8.043059e-6, -2.991300e-6, 1.473654e-7, -3.154542e-7, 0.0e0},
667 { 3.699128e-6, -3.316126e-6, 2.901257e-7, 3.407826e-7, 0.0e0},
668 { 2.550120e-6, -1.241123e-6, 9.901116e-8, 2.210482e-7, 0.0e0},
669 {-6.351059e-7, 2.341650e-6, 1.061492e-6, 2.878231e-7, 0.0e0}
679 const cxdouble ccsec3 = -7.757020e-08;
681 const cxdouble ccsec[][3] = {
682 {1.289600e-06, 5.550147e-01, 2.076942e+00},
683 {3.102810e-05, 4.035027e+00, 3.525565e-01},
684 {9.124190e-06, 9.990265e-01, 2.622706e+00},
685 {9.793240e-07, 5.508259e+00, 1.559103e+01}
693 const cxdouble dcsld = 1.990987e-07;
694 const cxdouble ccsgd = 1.990969e-07;
702 const cxdouble cckm = 3.122140e-05;
703 const cxdouble ccmld = 2.661699e-06;
704 const cxdouble ccfdi = 2.399485e-07;
714 const cxdouble dcargm[][2] = {
715 {5.1679830e+00, 8.3286911095275e+03},
716 {5.4913150e+00, -7.2140632838100e+03},
717 {5.9598530e+00, 1.5542754389685e+04}
726 const cxdouble ccampm[][4] = {
727 { 1.097594e-01, 2.896773e-07, 5.450474e-02, 1.438491e-07},
728 {-2.223581e-02, 5.083103e-08, 1.002548e-02, -2.291823e-08},
729 { 1.148966e-02, 5.658888e-08, 8.249439e-03, 4.063015e-08}
737 const cxdouble ccpamv[4] = {
749 const cxdouble dc1mme = 0.99999696;
752 register cxint k = 0;
753 register cxint n = 0;
766 cxdouble dlocal = 0.;
769 cxdouble pertld = 0.;
771 cxdouble pertrd = 0.;
773 cxdouble pertpd = 0.;
779 cxdouble dparam = 0.;
787 cxdouble d1pdro = 0.;
790 cxdouble dsinls = 0.;
791 cxdouble dcosls = 0.;
805 cxdouble dcosep = 0.;
806 cxdouble dsinep = 0.;
811 cxdouble sn[4] = {0., 0., 0., 0.};
812 cxdouble sinlp[4] = {0., 0., 0., 0.};
813 cxdouble coslp[4] = {0., 0., 0., 0.};
814 cxdouble forbel[7] = {0., 0., 0., 0., 0., 0., 0.};
818 memset(sorbel, 0,
sizeof sorbel);
826 dt = (dje - dct0) / dcjul;
836 for (k = 0; k < 8; k++) {
838 dlocal = fmod(dcfel[k][0] + dt * dcfel[k][1] + dtsq * dcfel[k][2],
846 forbel[k - 1] = dlocal;
851 deps = fmod(dceps[0] + dt * dceps[1] + dtsq * dceps[2], RV_D2PI);
853 for (k = 0; k < 17; k++) {
855 sorbel[k] = fmod(ccsel[k][0] + t * ccsel[k][1] + tsq * ccsel[k][2],
865 for (k = 0; k < 4; k++) {
867 a = fmod(ccsec[k][1] + t * ccsec[k][2], RV_D2PI);
877 pertl = ccsec[0][0] * sn[0] + ccsec[1][0] * sn[1] +
878 (ccsec[2][0] + t * ccsec3) * sn[2] + ccsec[3][0] * sn[3];
884 for (k = 0; k < 15; k++) {
886 a = fmod(dcargs[k][0] + dt * dcargs[k][1], RV_D2PI);
889 pertl += (ccamps[k][0] * cosa + ccamps[k][1] * sina);
890 pertr += (ccamps[k][2] * cosa + ccamps[k][3] * sina);
896 pertld += ((ccamps[k][1] * cosa - ccamps[k][0] * sina) * ccamps[k][4]);
897 pertrd += ((ccamps[k][3] * cosa - ccamps[k][2] * sina) * ccamps[k][4]);
906 esq = sorbel[0] * sorbel[0];
909 twoe = sorbel[0] + sorbel[0];
910 twog = forbel[0] + forbel[0];
911 phi = twoe * ((1.0 - esq * (1.0 / 8.0)) * sin (forbel[0]) +
912 sorbel[0] * (5.0 / 8.0) * sin (twog) +
913 esq * 0.5416667 * sin (forbel[0] + twog));
917 dpsi = dparam / (1. + sorbel[0] * cos_f);
918 phid = twoe * ccsgd * ((1.0 + esq * 1.50) * cos_f +
919 sorbel[0] * (1.250 - sin_f * sin_f * 0.50));
920 psid = ccsgd * sorbel[0] * sin_f / sqrt(param);
928 drd = d1pdro * (psid + dpsi * pertrd);
929 drld = d1pdro * dpsi * (dcsld + phid + pertld);
930 dtl = fmod(dml + phi + pertl, RV_D2PI);
933 dxhd = drd * dcosls - drld * dsinls;
934 dyhd = drd * dsinls + drld * dcosls;
947 for (k = 0; k < 3; k++) {
949 a = fmod(dcargm[k][0] + dt * dcargm[k][1], RV_D2PI);
952 pertl += ccampm[k][0] * sina;
953 pertld += ccampm[k][1] * cosa;
954 pertp += ccampm[k][2] * cosa;
955 pertpd -= ccampm[k][3] * sina;
964 tl = forbel[1] + pertl;
967 sigma = cckm / (1. + pertp);
968 a = sigma * (ccmld + pertld);
970 dxhd = dxhd + a * sinlm + b * coslm;
971 dyhd = dyhd - a * coslm + b * sinlm;
972 dzhd = -sigma * ccfdi * cos(forbel[2]);
979 dxbd = dxhd * dc1mme;
980 dybd = dyhd * dc1mme;
981 dzbd = dzhd * dc1mme;
983 for (k = 0; k < 4; k++) {
985 plon = forbel[k + 3];
986 pomg = sorbel[k + 1];
987 pecc = sorbel[k + 9];
988 tl = fmod(plon + 2.0 * pecc * sin (plon - pomg), RV_D2PI);
991 dxbd = dxbd + ccpamv[k] * (sinlp[k] + pecc * sin(pomg));
992 dybd = dybd - ccpamv[k] * (coslp[k] + pecc * cos(pomg));
993 dzbd = dzbd - ccpamv[k] * sorbel[k + 13] * cos(plon - sorbel[k + 5]);
1004 dyahd = dcosep * dyhd - dsinep * dzhd;
1005 dzahd = dsinep * dyhd + dcosep * dzhd;
1006 dyabd = dcosep * dybd - dsinep * dzbd;
1007 dzabd = dsinep * dybd + dcosep * dzbd;
1026 cxdouble deqdat = (dje - dct0 - dcbes) / dctrop + dc1900;
1028 cpl_matrix* prec = slaPrecession(deqdat, deq);
1031 for (n = 0; n < 3; n++) {
1034 dxhd * cpl_matrix_get(prec, 0, n) +
1035 dyahd * cpl_matrix_get(prec, 1, n) +
1036 dzahd * cpl_matrix_get(prec, 2, n);
1039 dxbd * cpl_matrix_get(prec, 0, n) +
1040 dyabd * cpl_matrix_get(prec, 1, n) +
1041 dzabd * cpl_matrix_get(prec, 2, n);
1044 cpl_matrix_delete(prec);
1092 cxdouble jdate, cxdouble longitude,
1093 cxdouble latitude, cxdouble elevation,
1094 cxdouble ra, cxdouble dec,
1100 const cxdouble aukm = 1.4959787e08;
1106 cxdouble dc[3] = {0., 0., 0.};
1107 cxdouble dcc[3] = {0., 0., 0.};
1108 cxdouble hv[3] = {0., 0., 0.};
1109 cxdouble bv[3] = {0., 0., 0.};
1110 cxdouble _long = longitude * RV_DD2R;
1111 cxdouble _lat = latitude * RV_DD2R;
1112 cxdouble _ra = ra * 15.0 * RV_DD2R;
1113 cxdouble _dec = dec * RV_DD2R;
1114 cxdouble st = sideral_time(jdate, _long);
1116 cpl_matrix* precession = NULL;
1123 eqt = (jdate - dct0 - dcbes) / dctrop + dc1900;
1125 dc[0] = cos(_ra) * cos(_dec);
1126 dc[1] = sin(_ra) * cos(_dec);
1129 precession = slaPrecession(equinox, eqt);
1131 for (i = 0; i < 3; ++i) {
1134 dc[0] * cpl_matrix_get(precession, i, 0) +
1135 dc[1] * cpl_matrix_get(precession, i, 1) +
1136 dc[2] * cpl_matrix_get(precession, i, 2);
1140 cpl_matrix_delete(precession);
1146 cxdouble darg = dcc[1] / dcc[0];
1172 dec2 = asin(dcc[2]);
1187 rv->gc = geo_correction(_lat, elevation, dec2, -ha);
1195 earth_velocity (jdate, eqt, hv, bv);
1206 for (i = 0; i < 3; ++i) {
1207 rv->bc += bv[i] * dcc[i] * aukm;
1208 rv->hc += hv[i] * dcc[i] * aukm;