void SolarRadiationPressure::doCompute(UTCTime utc, EarthBody& rb, Spacecraft& sc)
{
crossArea = sc.getDragArea();
dryMass = sc.getDryMass();
reflectCoeff = sc.getReflectCoeff();
Vector<double> r_sun = ReferenceFrames::getJ2kPosition(utc.asTDB(),SolarSystem::Sun);
Vector<double> r_moon = ReferenceFrames::getJ2kPosition(utc.asTDB(),SolarSystem::Moon);
// from km to m
r_sun = r_sun*1000.0;
r_moon = r_moon*1000.0;
// a
a = accelSRP(sc.R(),r_sun)*getShadowFunction(sc.R(),r_sun,r_moon,SM_CONICAL);
// da_dr reference to Montenbruck P248
// and it's in the same way as the gravitational attraction of the sun
da_dr.resize(3,3,0.0);
double au2 = ASConstant::AU * ASConstant::AU;
double factor = -1.0*reflectCoeff * (crossArea/dryMass) * ASConstant::P_Sol*au2;
Vector<double> d = sc.R() - r_sun;
double dmag = norm(d);
double dcubed = dmag * dmag *dmag;
Vector<double> temp1 = d / dcubed; // detRJ/detRJ^3
double muod3 = factor / dcubed;
double jk = 3.0 * muod3/dmag/dmag;
double xx = d(0);
double yy = d(1);
double zz = d(2);
da_dr(0,0) = jk * xx * xx - muod3;
da_dr(0,1) = jk * xx * yy;
da_dr(0,2) = jk * xx * zz;
da_dr(1,0) = da_dr(0,1);
da_dr(1,1) = jk * yy * yy - muod3;
da_dr(1,2) = jk * yy * zz;
da_dr(2,0) = da_dr(0,2);
da_dr(2,1) = da_dr(1,2);
da_dr(2,2) = jk * zz * zz - muod3;
// da_dv
da_dv.resize(3,3,0.0);
// da_dp
dadcr.resize(3,0.0);
dadcr = a /reflectCoeff;
da_dcr(0,0) = dadcr(0);
da_dcr(1,0) = dadcr(1);
da_dcr(2,0) = dadcr(2);
} // End of method 'SolarRadiationPressure::doCompute()'
/* Call the relevant methods to compute the acceleration.
* @param t Time reference class
* @param bRef Body reference class
* @param sc Spacecraft parameters and state
* @return the acceleration [m/s^s]
*/
void MoonForce::doCompute(UTCTime utc, EarthBody& rb, Spacecraft& sc)
{
/* Oliver P69 and P248
* a = GM*( (s-r)/norm(s-r)^3 - s/norm(s)^3 )
*
* da/dr = -GM*( I/norm(r-s)^3 - 3(r-s)transpose(r-s)/norm(r-s)^5)
*/
Vector<double> r_moon = ReferenceFrames::getJ2kPosition(utc.asTDB(), SolarSystem::Moon);
r_moon = r_moon * 1000.0; // from km to m
Vector<double> d = sc.R() - r_moon;
double dmag = norm(d);
double dcubed = dmag * dmag *dmag;
Vector<double> temp1 = d / dcubed; // detRJ/detRJ^3
double smag = norm(r_moon);
double scubed = smag * smag * smag;
Vector<double> temp2 = r_moon / scubed; // Rj/Rj^3
Vector<double> sum = temp1 + temp2;
a = sum * (-mu); // a
// da_dr
da_dr.resize(3,3,0.0);
double muod3 = mu / dcubed;
double jk = 3.0 * muod3/dmag/dmag;
double xx = d(0);
double yy = d(1);
double zz = d(2);
da_dr(0,0) = jk * xx * xx - muod3;
da_dr(0,1) = jk * xx * yy;
da_dr(0,2) = jk * xx * zz;
da_dr(1,0) = da_dr(0,1);
da_dr(1,1) = jk * yy * yy - muod3;
da_dr(1,2) = jk * yy * zz;
da_dr(2,0) = da_dr(0,2);
da_dr(2,1) = da_dr(1,2);
da_dr(2,2) = jk * zz * zz - muod3;
// da_dv
da_dv.resize(3,3,0.0);
//da_dp
} // End of method 'MoonForce::doCompute()'
/* Ocean pole tide to normalized earth potential coefficients
*
* @param mjdUtc UTC in MJD
* @param dC Correction to normalized coefficients dC
* @param dS Correction to normalized coefficients dS
* C20 C21 C22 C30 C31 C32 C33 C40 C41 C42 C43 C44
*/
void EarthOceanTide::getOceanTide(double mjdUtc, double dC[], double dS[] )
{
try
{
loadTideFile(fileName,maxN,minX);
}
catch (...)
{
// faild to get the ocean tide model
// return zeros
const int n = (maxN-1)*(maxN+4)/2;
for(int i=0;i<n;i++)
{
dC[i] = 0.0;
dS[i] = 0.0;
}
return;
}
UTCTime utc;
utc=mjdUtc;
// CC PURPOSE : COMPUTE DOODSON'S FUNDAMENTAL ARGUMENTS (BETA)
// CC AND FUNDAMENTAL ARGUMENTS FOR NUTATION (FNUT)
double BETA[6]={0.0};
double FNUT[5] ={0.0};
ReferenceFrames::doodsonArguments(utc.asUT1(), utc.asTT(), BETA,FNUT);
for(int i=0;i<tideData.NTACT;i++)
{
int N= tideData.NM[i][0];
int M= tideData.NM[i][1];
if(tideData.NM[i][0]>maxN) continue;
double delta = M ? 0 : 1;
double FNM = 4.0*ASConstant::PI*G*RHOW/GE* std::sqrt(FAC[N+M]/FAC[N-M]/(2.0*N+1.0)/(2.0-delta))
*(1.0+tideData.KNMP[N-1])/(2.0*N+1)/100.0;
double ARG =0;
for(int j=0;j<6;j++)
{
ARG +=tideData.NDOD[i][j]*BETA[j];
}
double CARG = std::cos(ARG);
double SARG = std::sin(ARG);
int index=N*(N+1)/2-3+M;
dC[index]=dC[index]
+FNM*(
(tideData.CSPM[i][0]+tideData.CSPM[i][2])*CARG
+(tideData.CSPM[i][1]+tideData.CSPM[i][3])*SARG);
dS[index]=dS[index]
+FNM*(
(tideData.CSPM[i][1]-tideData.CSPM[i][3])*CARG
-(tideData.CSPM[i][0]-tideData.CSPM[i][2])*SARG);
}
} // End of method 'EarthOceanTide::getOceanTide()'
/* Abstract class requires the subclass to compute the atmospheric density.
* @param utc epoch in UTC
* @param rb EarthRef object.
* @param r Position vector.
* @param v Velocity vector
* @return Atmospheric density in kg/m^3
*/
double HarrisPriesterDrag::computeDensity( UTCTime utc,
EarthBody& rb,
Vector<double> r,
Vector<double> v)
{
double density = 0.0;
// Get the J2000 to TOD transformation
Matrix<double> N = ReferenceFrames::J2kToTODMatrix(utc);
// Debuging
/*
double nn[3][3]={ {0.9999994803, -0.0009350126, -0.0004063480},
{0.0009349995, 0.9999995624, -0.0000322758},
{0.0004063780, 0.0000318958, 0.9999999169}};
N = &nn[0][0];
cout<<fixed<<setprecision(6);
//cout<<workingDens<<endl;
*/
// Transform r from J2000 to TOD
Vector<double> r_tod = N * r;
//* Satellite true altitude
Position pos(r_tod(0), r_tod(1), r_tod(2), Position::Cartesian);
double alt = pos.getAltitude()/1000.0; // km
if ( alt >= upper_limit || alt <= lower_limit )
{
//return 0.0;
string msg = "HarrisPriesterDrag is good for 100.0 km t0 2000.0 km"
+ string("the altitude you try is ")
+ StringUtils::asString(alt) + " km!";
Exception e(msg);
//GPSTK_THROW(e);
}
Vector<double> r_Sun = ReferenceFrames::getJ2kPosition( utc.asTDB(),
SolarSystem::Sun);
// get coefficients for this F107
//updateF107(std::pow(149597870.0/norm(r_Sun),2)*dailyF107);
updateF107(averageF107);
// Debuging
/*
alt = 360.01323431364915;
cout<<r_Sun<<endl;
double rr[3] = {4.87202078904524E7, 1.318213179104784E8, 5.71498291483194E7};
r_Sun = &rr[0];
*/
double ra_Sun = std::atan2( r_Sun(1), r_Sun(0));
double dec_Sun = std::atan2( r_Sun(2),
std::sqrt(
std::pow(r_Sun(0),2) + std::pow(r_Sun(1),2)
) );
//* Unit vector u towards the apex of the diurnal bulge
//* in inertial geocentric coordinates
double c_dec = std::cos(dec_Sun);
Vector<double> u(3,0.0); // Apex of diurnal bulge
u(0) = c_dec * std::cos(ra_Sun + ra_lag);
u(1) = c_dec * std::sin(ra_Sun + ra_lag);
u(2) = std::sin(dec_Sun);
//* Cosine of half angle between satellite position vector and
//* apex of diurnal bulge
double c_psi2 = (0.5 + 0.5 * dot(r,u)/norm(r));
int ih = 0;
// index search
for(int i = 0; i < 59-1; i++)
{
if(alt >= workingDens[i][0] && alt < workingDens[i+1][0])
{
ih = i;
break;
}
}
// exponential density interpolation
double h_min = (workingDens[ih][0] - workingDens[ih+1][0])
/ std::log(workingDens[ih+1][1] / workingDens[ih][1]);
double h_max = (workingDens[ih][0] - workingDens[ih+1][0])
/ std::log(workingDens[ih+1][2]
/ workingDens[ih][2]);
double d_min = workingDens[ih][1]*std::exp((workingDens[ih][0]-alt)/h_min);
double d_max = workingDens[ih][2]*std::exp((workingDens[ih][0]-alt)/h_max);
// Density computation
//Use 2 for low inclination orbits and 6 for polar orbits
// Reference Montenbruck P90
double n_prm = 2.0;
Vector<double> h = cross(r,v);
double inc = std::acos(h(2) / norm(h));
n_prm = 2.0 + inc * 8.0 / ASConstant::PI;
density = d_min + (d_max - d_min) * std::pow(c_psi2,n_prm / 2.0);
return density * 1.0e-9; //[kg/m^3]
} // End of method 'HarrisPriesterDrag::computeDensity()'
