Skip to content

C
common
Commit 22ec10db authored by Alexander Lapshin's avatar Alexander Lapshin
Browse files
Options

.

parent 4501f8f4
Hide whitespace changes
Inline Side-by-side
Showing with 122 additions and 330 deletions
utils/AscNodeOrbit.cpp
View file @ 22ec10db
......@@ -24,6 +24,9 @@ AscNodeOrbit::AscNodeOrbit(const PhasePoint6D& p)
void AscNodeOrbit::calc_asc_node(Propagator& prop, const TimeJD& date, const bool vzmode, const bool gcs_node, PhasePoint6D& result) const
{
// std::string date_s = DateToStr(date);
// std::cout << date_s << "\n";
if (vzmode) {
//if(!GetAscNodeYZ(*prop.GetPredictor()->GetPredictor(), prop.GetPredictor()->GetTime(), result, GetZ, GetVZ)){
// return false;
......@@ -36,11 +39,15 @@ void AscNodeOrbit::calc_asc_node(Propagator& prop, const TimeJD& date, const boo
result = ascNodeOrbit.GetPhasePointJ2000();
//std::string date_s = DateToStr(result.T);
//std::cout << date_s << "\n";
if (DaysInterval(date, result.T) * 86400 > 1) {
// TODO sometimes date lower result.T less than 1 sec
//SInitOrbit ascNodeOrbit;
//if (!jump_arg_of_lat(date, prop, 0, gcs_node, ascNodeOrbit)) {
// throw Exp() << "internal error in ascending node method 1";
// throw Exp() << "internal error in ascending node method 2";
//}
throw Exp() << "internal error in ascending node method 2 " << DaysInterval(date, result.T)*86400;
}
......@@ -60,14 +67,19 @@ void AscNodeOrbit::calc_desc_node(Propagator& prop, const TimeJD& date, const bo
// return false;
//};
SInitOrbit ascNodeOrbit;
if (!jump_arg_of_lat(date, prop, 180 * deg2rad, gcs_node, ascNodeOrbit)) {
SInitOrbit descNodeOrbit;
if (!jump_arg_of_lat(date, prop, 180 * deg2rad, gcs_node, descNodeOrbit, 1)) {
throw Exp() << "internal error in descending node method 1";
}
result = ascNodeOrbit.GetPhasePointJ2000();
if (date < result.T) {
result = descNodeOrbit.GetPhasePointJ2000();
//std::cout << DateToStr(date) << " " << DateToStr(descNodeOrbit.GetPhasePointJ2000().T) << "\n" ;
if (DaysInterval(result.T, date) * 86400 > 1) {
// if (!jump_arg_of_lat(date, prop, 180 * deg2rad, gcs_node, descNodeOrbit, 1)) {
// throw Exp() << "internal error in descending node method 2";
// }
// std::cout << DateToStr(date) << " " << DateToStr(descNodeOrbit.GetPhasePointJ2000().T) << "\n";
throw Exp() << "internal error in descending node method 2 " << DaysInterval(date, result.T) * 86400;
}
......@@ -86,8 +98,9 @@ void AscNodeOrbit::calculate(Propagator& prop, const TimeJD& date, const bool vz
PhasePoint6D j2000AscNodePoint;
calc_asc_node(prop, date, vzmode, gcs_node, j2000AscNodePoint);
calc_draconic_period(prop, j2000AscNodePoint.T, vzmode, gcs_node, m_drperiod, m_drperiodDt);
m_date = j2000AscNodePoint.T;
if (calc_params) {
calculate_params(j2000AscNodePoint);
}
......@@ -134,73 +147,78 @@ double AscNodeOrbit::get_kep_time(double TA, double e, double w) const
return (E - e * sin(E)) / w;
}
bool AscNodeOrbit::jump_arg_of_lat(const TimeJD& date, Propagator& prop, double dstU, bool gcs_node, SInitOrbit& result) const
double AscNodeOrbit::get_step(const Kepler& kep, double dst_u, bool positive) const
{
return get_step(kep.GetPeriod(), kep.GetArgP(), kep.GetEcc(), kep.U, dst_u, positive);
}
double AscNodeOrbit::get_step(double period, double arg_of_per, double e, double u, double dst_u, bool positive) const
{
double w = 86400 / period / 1000 * Pi2 / 1440 / 60;
double t = get_kep_time((u - arg_of_per), e, w);
double t2 = get_kep_time((dst_u - arg_of_per), e, w);
double step = t2 - t;
if (step > period * 1000 / 2) {
step = step - period * 1000;
}
else if (step < -period * 1000 / 2) {
step = period * 1000 + step;
}
if(positive && step < 0) {
step += period * 1000;
}
return step;
}
bool AscNodeOrbit::jump_arg_of_lat(const TimeJD& date, Propagator& prop, double dstU, bool gcs_node, SInitOrbit& result, int type) const
{
const int maxInter = 20;
int iter = 0;
const bool nearestAscendingNode = false;
const double u_tol = 0.001;
const double time_tol = 0.001;
int iter = 0;
PhasePoint6D p;
prop.Propagate(date, p);
if (!nearestAscendingNode) {
Kepler kep;
PhasePoint6D pt = p;
//if (gcs_node) {
// pt.Convert(METEFrame(pt.T));
//}
kep.SetXYZ(pt.CoordsVel);
prop.Propagate(kep.GetTimeFromAN(pt.T), p);
//PhasePoint6D pt = p;
//if (gcs_node) {
// pt.Convert(METEFrame(pt.T));
//}
if(type == 1) {
Kepler kep;
kep.SetXYZ(p.CoordsVel);
const double step = get_step(kep, dstU, true);
prop.Propagate(ShiftDate(p.T, step), p);
}else {
Kepler kep;
kep.SetXYZ(p.CoordsVel);
prop.Propagate(kep.GetTimeFromAN(p.T), p);
}
while (true) {
double u;
double e;
double arg_of_per;
double period;
//std::string date_s = DateToStr(p.T);
//std::cout << date_s << "\n";
// std::cout << DateToStr(p.T) << "\n";
Kepler kep;
if (gcs_node) {
PhasePoint6D gcs = p;
gcs.Convert(ECIFrame(gcs.T));
Kepler gcs_kep;
gcs_kep.SetXYZ(gcs.CoordsVel);
u = gcs_kep.U;
arg_of_per = gcs_kep.GetArgP();
e = gcs_kep.GetEcc();
period = gcs_kep.GetPeriod();
kep.SetXYZ(gcs.CoordsVel);
}
else {
Kepler kep;
kep.SetXYZ(p.CoordsVel);
u = kep.U;
e = kep.GetEcc();
arg_of_per = kep.GetArgP();
period = kep.GetPeriod();
}
if (fabs(Distance(0, u, 0, dstU)) * rad2deg < u_tol) {
if (fabs(Distance(0, kep.U, 0, dstU)) * rad2deg < u_tol) {
break;
}
double w = 86400 / period / 1000 * Pi2 / 1440 / 60;
double t = get_kep_time((u - arg_of_per), e, w);
double t2 = get_kep_time((dstU - arg_of_per), e, w);
double step = t2 - t;
if (step > period * 1000 / 2) {
step = step - period * 1000;
}
else if (step < -period * 1000 / 2) {
step = period * 1000 - step;
}
double step = get_step(kep, dstU, false);
if (fabs(step) < time_tol) {
break;
}
......@@ -211,7 +229,7 @@ bool AscNodeOrbit::jump_arg_of_lat(const TimeJD& date, Propagator& prop, double
prop.Propagate(ShiftDate(p.T, step), p);
}
prop.Propagate(p.T, result);
return true;
......
utils/AscNodeOrbit.h
View file @ 22ec10db
......@@ -34,7 +34,9 @@ private:
void calc_draconic_period(Propagator& prop, const TimeJD& asc_node_date, const bool vzmode, const bool gcs_node, double& dracp, double& vdracp) const;
double get_kep_time(double TA, double e, double w) const;
static KepData get_kep_data(const Vect6& pos);
bool jump_arg_of_lat(const TimeJD& date, Propagator& prop, double dstU, bool gcs_node, SInitOrbit& result) const;
bool jump_arg_of_lat(const TimeJD& date, Propagator& prop, double dstU, bool gcs_node, SInitOrbit& result, int type = 0) const;
double get_step(double period, double arg_of_per, double e, double u, double dst_u, bool positive) const;
double get_step(const Kepler& kep, double dst_u, bool positive) const;
double m_latitude;
double m_longitude;
......
utils/ephi.h
View file @ 22ec10db
......@@ -62,8 +62,4 @@ struct ei
};
int cephinit(char* str);
int cinterp_pvac(ei& ephi,
double jdint, double seceph,
double outvec[6], double backvec[6],
double aberout[3], double aberback[3],
double* corr12, double* corr23, int fvel, int* ircode);
int cinterp_pvac(ei& ephi, double jdint, double seceph, double outvec[6], int fvel, int* ircode);
utils/ephinterp.cpp
View file @ 22ec10db
......@@ -4,7 +4,22 @@
int debug = 0;
void hermite(int ityp, const double* x, const double* y, const double* z, int nmax, int nval, double xp, double* yp, double* zp, int* ircode);
int bs_lower_bound(double a[], int n, double x) {
int l = 0;
int h = n; // Not n - 1
while (l < h) {
int mid = (l + h) / 2;
if (x <= a[mid]) {
h = mid;
}
else {
l = mid + 1;
}
}
return l;
}
void hermite(int index, int ityp, const double* x, const double* y, const double* z, int nmax, int nval, double xp, double* yp, double* zp, int* ircode);
/*-----------------------------------------------------------------------
void cinterp_pvac (jdint, seceph, outvec, backvec,
......@@ -27,269 +42,46 @@ Author: R. L. Ricklefs / UT CSR
Date: 1 May, 2003
History:
-----------------------------------------------------------------------*/
int
cinterp_pvac(ei& ephi,
double jdint, double seceph,
double outvec[6], double backvec[6],
double aberout[3], double aberback[3],
double* corr12, double* corr23, int fvel, int* ircode)
int cinterp_pvac(
ei& ephi,
double jdint,
double seceph,
double outvec[6],
int fvel,
int* ircode)
{
double itime, z, vz; /* z is dummy for derivative field */
int j;
itime = ((seceph / 86400.e0 - ephi.jdf[0]) + (jdint - ephi.jdi[0])) * 86400.e0;
/**
debug= 1;
printf("itime %f seceph %f jfd[0] %f jding %f jdi[0] %f\n",
itime,seceph,ephi.jdf[0],jdint,ephi.jdi[0]);
**/
hermite(1, ephi.jds, ephi.c12, &z, ephi.nv, 10, itime, corr12, &vz, ircode);
if (*ircode != 0) {
if (debug) {
printf("readi jds(0): %f jds(ephi.nv-1): %f %d\n", ephi.jds[0], ephi.jds[ephi.nv - 1], ephi.nv);
printf("itime= %f\n", itime);
printf("hermite error: c12\n");
}
return *ircode;
}
hermite(1, ephi.jds, ephi.c23, &z, ephi.nv, 10, itime, corr23, &vz, ircode);
if (*ircode != 0) {
if (debug) {
printf("hermite error: c23\n");
}
return *ircode;
}
/*for (j = 0; j < 3; j++)
{
hermite (1, ephi.jds, &ephi.aberoutv[j][0], &z, ephi.nv, 10, itime,
&aberout[j], &vz, ircode);
if (*ircode != 0)
{
if (debug)
printf ("hermite error: aberout %d\n", j);
return;
}
hermite (1, ephi.jds, &ephi.aberbackv[j][0], &z, ephi.nv, 10, itime,
&aberback[j], &vz, ircode);
if (*ircode != 0)
{
if (debug)
printf ("hermite error: aberback %d\n", j);
return;
}
}*/
/* 3 or 6 elements? Handle velocity if present */
int pindex = bs_lower_bound((double*)ephi.jds, ephi.nv, itime);
for (j = 0; j < 6; j++) {
/* Out */
/* Position */
if (j < 3) {
if (fabs(ephi.outv[j + 3][0]) > 1.e-10) {
/* Use velocities if they exist (hermite) */
hermite(2, ephi.jds, &ephi.outv[j][0], &ephi.outv[j + 3][0], ephi.nv, 10, itime, &outvec[j], &vz, ircode);
hermite(pindex, 2, ephi.jds, &ephi.outv[j][0], &ephi.outv[j + 3][0], ephi.nv, 10, itime, &outvec[j], &vz, ircode);
}
else {
/* Otherwise, ignore them (fvel = -1) or create them (fvel = 1) */
hermite(-fvel, ephi.jds, &ephi.outv[j][0], &z, ephi.nv, 10, itime, &outvec[j], &outvec[j + 3], ircode);
hermite(pindex, -fvel, ephi.jds, &ephi.outv[j][0], &z, ephi.nv, 10, itime, &outvec[j], &outvec[j + 3], ircode);
}
}
/* Velocity */
/* Interpolate if they exist */
/* Velocity */
/* Interpolate if they exist */
else if (fabs(ephi.outv[j][0]) > 1.e-10) {
hermite(1, ephi.jds, &ephi.outv[j][0], &z, ephi.nv, 10, itime, &outvec[j], &vz, ircode);
hermite(pindex, 1, ephi.jds, &ephi.outv[j][0], &z, ephi.nv, 10, itime, &outvec[j], &vz, ircode);
}
if (*ircode != 0) {
if (debug) {
printf("hermite error: outvec %d\n", j);
}
return *ircode;
}
/* Back */
/* Position */
/*if (j < 3)
{
if (fabs (ephi.backv[j + 3][0]) > 1.e-10)
{
/* Use velocities if they exist (hermite) * /
hermite (2, ephi.jds, &ephi.backv[j][0], &ephi.backv[j + 3][0],
ephi.nv, 10, itime, &backvec[j], &vz, ircode);
}
else
{
/* Otherwise, ignore them (fvel = -1) or create them (fvel = 1) * /
hermite (-fvel, ephi.jds, &ephi.backv[j][0], &z, ephi.nv,
10, itime, &backvec[j], &backvec[j + 3], ircode);
}
}*/
/* Velocity */
/* Interpolate if they exist */
/*else if (fabs (ephi.backv[j][0]) > 1.e-10)
{
hermite (1, ephi.jds, &ephi.backv[j][0], &z, ephi.nv, 10, itime,
&backvec[j], &vz, ircode);
}*
if (*ircode != 0)
{
if (debug)
printf ("hermite error: backvec %d\n", j);
return;
}*/
//if (debug){
// printf ("readi-> %d %f %f\n", j, outvec[j], backvec[j]);
//}
}
return 0;
}
/*-----------------------------------------------------------------------
void interp_off (jdint, seceph, offsetout,offsetback,
ircode)
Interpolate offset from center of main body from cpf file arrays
stored in memory.
jdint Julian date (integer day)
seceph Julian day (seconds)
offsetout outbound (or bounce) offset in m
offsetback inbound offset in m
ircode error code: 0=OK, 1,2=error (time out of range)
Author: R. L. Ricklefs / UT CSR
Date: 1 May, 2004
History:
-----------------------------------------------------------------------*/
/*void
cinterp_off (ei& ephi,
double jdint, double seceph,
double offsetout[6], double offsetback[6], int *ircode)
{
double itime, z, vz; /* z is dummy for derivative field * /
int debug = 0;
int j;
itime =
((seceph / 86400.e0 - ephi.offsetjdf[0]) +
(jdint - ephi.offsetjdi[0])) * 86400.e0;
for (j = 0; j < 3; j++)
{
hermite (1, ephi.offsetjds, &ephi.offsetoutv[j][0], &z,
ephi.nvoff, 10, itime, &offsetout[j], &vz, ircode);
if (*ircode != 0)
{
if (debug)
printf ("hermite error: offsetout %d\n", j);
return;
}
hermite (1, ephi.offsetjds, &ephi.offsetbackv[j][0], &z,
ephi.nvoff, 10, itime, &offsetback[j], &vz, ircode);
if (*ircode != 0)
{
if (debug)
printf ("hermite error: offsetback %d\n", j);
return;
}
}
}*/
//void
//cinterp_rot (const eh& ephihdr, ei& ephi,
// double jdint, double seceph,
// int *arottype, double *agast, double arotang[3], int *ircode)
//{
// double itime, z, vz; /* z is dummy for derivative field */
// int debug = 0;
// int j;
//
// itime =
// ((seceph / 86400.e0 - ephi.rotjdf[0]) +
// (jdint - ephi.rotjdi[0])) * 86400.e0;
// *arottype = ephihdr.rot_type;
//
// hermite (1, ephi.rotjds, ephi.gast, &z,
// ephi.nvrot, 10, itime, agast, &vz, ircode);
// if (*ircode != 0)
// {
// if (debug)
// printf ("hermite error: gast\n");
// return;
// }
//
// for (j = 0; j < 3; j++)
// {
// hermite (1, ephi.rotjds, &ephi.rotangv[j][0], &z,
// ephi.nvrot, 10, itime, &arotang[j], &vz, ircode);
// if (*ircode != 0)
// {
// if (debug)
// printf ("hermite error: rotang %d\n", j);
// return;
// }
// }
// if (debug)
// printf ("rot: %12.6f %12.6f %12.6f\n",
// arotang[0], arotang[1], arotang[2]);
//}
/*-----------------------------------------------------------------------
void interp_rot (jdint, seceph, arottype, agast, arotang,
ircode)
Interpolate rotation angles (euler or north pole RA, Dec, and W)
from cpf file arrays stored in memory.
jdint Julian date (integer day)
seceph Julian day (seconds)
arotype Rotation type (int): 0=n/a; 1=Lunar euler; 2=ra/dec/W
agast Greenwich apparent sidereal time
arotang x, y, z, or ra, dec, w (3 elements, d.p. in deg)
ircode error code: 0=OK, 1,2=error (time out of range)
Author: R. L. Ricklefs / UT CSR
Date: 1 May, 2004
History:
-----------------------------------------------------------------------*/
//void
//cinterp_eop (double jdint, double seceph,
// double x, double y, double dutc, int *ircode)
//{
// double itime, z, vz; /* z is dummy for derivative field */
// int debug = 0;
// int j;
//
// itime =
// ((seceph / 86400.e0 - ephi.jdf[0]) + (jdint - ephi.jdi[0])) * 86400.e0;
//
// hermite (1, ephi.eopjds, &ephi.eopxy[0][0], &z,
// ephi.nveop, 10, itime, &x, &vz, ircode);
// if (*ircode != 0)
// {
// if (debug)
// printf ("hermite error: eop x\n");
// return;
// }
// hermite (1, ephi.eopjds, &ephi.eopxy[1][0], &z,
// ephi.nveop, 10, itime, &y, &vz, ircode);
// if (*ircode != 0)
// {
// if (debug)
// printf ("hermite error: eop y\n");
// return;
// }
// int ircode;
// hermite (1, ephi.eopjds, &ephi.eopdutc, &z, ephi.nveop, 10, itime, &dutc, ircode);
// if (*ircode != 0)
// {
// if (debug)
// printf ("hermite error: eop dutc\n");
// return;
// }
//}
utils/hermite.cpp
View file @ 22ec10db
#include <stdio.h>
#include <stdlib.h>
/**
......@@ -39,58 +38,46 @@ C
C Modified: 25-JUL-2005 : Check boundaries
C
**/
void
hermite(int ityp, const double* x, const double* y, const double* z, int nmax, int nval, double xp, double* yp, double* zp, int* ircode)
void hermite(int index, int ityp, const double* x, const double* y, const double* z, int nmax, int nval, double xp, double* yp, double* zp, int* ircode)
{
int i, i0, j, k, n;
int debug = 0;
int i0, j, k, n;
double pj, sk, vi, ui;
/**
for (i=0;i<nmax;i++)
{
printf("i %d x %f y %f\n",i,x[i],y[i]);
}
**/
*ircode = 0;
*yp = 0.e0;
*zp = 0.e0;
n = nval - 1;
/**printf("nval= %d nmax= %d xp= %f\n",nval,nmax,xp);**/
/* Look for given values to be used */
if (xp < x[0] || xp > x[nmax - 1]) {
*ircode = 2;
if (debug) {
//printf ("Her1 %d %f %f\n",i,x[i],xp);
}
return;
}
/* Look for given value immediately preceeding interpolation argument */
for (i = 0; i < nmax; i++) {
if (x[i] >= xp) {
*ircode = 0;
break;
}
}
// /* Look for given value immediately preceeding interpolation argument */
// for (i = 0; i < nmax; i++) {
// if (x[i] >= xp) {
// *ircode = 0;
// break;
// }
// }
int i = index;
/* Start index in vectors x,y,z */
i0 = i - (n + 1) / 2;
if (i0 < 0) {
if (debug) printf("Her2 %d %d %d %d %d\n", i, n, i0, i0 + n, nmax);
i0 = 0; /* or 1? */
*ircode = 1;
}
if (i0 + n > nmax) {
if (debug) printf("Her2 %d %d %d %d %d\n", i, n, i0, i0 + n, nmax);
i0 = nmax - n;
*ircode = 1;
}
/* Lagrange formula for polynomial interpolation */
if (abs(ityp) == 1) {
for (i = 0; i < n + 1; i++) /* i=0 or 1?? */ {
pj = 1.e0;
for (j = 0; j < n + 1; j++) {
......@@ -98,8 +85,7 @@ hermite(int ityp, const double* x, const double* y, const double* z, int nmax, i
}
*yp += y[i + i0] * pj;
}
/**printf("yp= %f\n",*yp);**/
/* COMPUTE DERIVATIVE OF THE LAGRANGE POLYNOMIAL */
if (ityp == -1) {
for (i = 0; i < n + 1; i++) /* i=0 or 1?? */ {
......@@ -123,7 +109,6 @@ hermite(int ityp, const double* x, const double* y, const double* z, int nmax, i
/* HERMITE INTERPOLATION (ADDITIONAL USE OF THE DERIVATIVES) */
}
else if (ityp == 2) {
/*printf("Vel:\n");*/
for (i = 0; i < n + 1; i++) {
sk = 0.e0;
for (k = 0; k < n + 1; k++) {
......@@ -138,7 +123,6 @@ hermite(int ityp, const double* x, const double* y, const double* z, int nmax, i
}
*yp += (y[i + i0] * vi + z[i + i0] * ui) * pj * pj;
/*printf("vel end: %d %d %f %f %f %f\n",i,i0,ui,pj,z[i+i0],*yp);*/
}
}
......
utils/interpolate.cpp
View file @ 22ec10db
......@@ -331,13 +331,12 @@ Vect6 XInterpolator::get_pos(const TimeJD& date) const
throw Exp() << "interpolation date out of range\n";
}
double outvec[6], backvec[6], aberout[3], aberback[3];
double relcorrout, relcorrback;
double outvec[6];
int fvel = 1;
int ircode;
double jdi = date.GetDays();
double sec = date.GetFraction() * 86400;
cinterp_pvac(*m_ephi, jdi, sec, outvec, backvec, aberout, aberback, &relcorrout, &relcorrback, fvel, &ircode);
cinterp_pvac(*m_ephi, jdi, sec, outvec, fvel, &ircode);
if (ircode != 0) {
throw Exp() << "interpolation fail\n";
......
utils/trj_inter.cpp
View file @ 22ec10db
......@@ -8,6 +8,7 @@ TrjInter::TrjInter(const OrbBlocksStore& orbit_processor)
bool TrjInter::Predict(const TimeJD& t, const IFrame& fr, Vect6& f)
{
Vect6 p = m_orbit_processor.get_pos(t);
p.x /= 1E6;
p.y /= 1E6;
p.z /= 1E6;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment