/* 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 SunForce::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_sun = ReferenceFrames::getJ2kPosition(utc.asTDB(), SolarSystem::Sun);
r_sun = r_sun * 1000.0; // from km to m
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 smag = norm(r_sun);
double scubed = smag * smag * smag;
Vector<double> temp2 = r_sun / scubed; // Rj/Rj^3
Vector<double> sum = temp1 + temp2;
a = sum * (-mu);
// 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 'SunForce::doCompute()'
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()'
// this is the real one
void RelativityEffect::doCompute(UTCTime utc, EarthBody& rb, Spacecraft& sc)
{
/* reference: Jisheng,Li P110 Bernese5 GENREL.f
a_rl = a_rl1 + a_rl2 + a_rl3
a_rl2 and a_rl3 are ignored for precise orbit determination
*/
const double GM = ASConstant::GM_Earth;
const double C = ASConstant::SPEED_OF_LIGHT;
Vector<double> r = sc.R();
Vector<double> v = sc.V();
double beta = 1.0;
double gama = 1.0;
double c2 = C * C;
double r2 = dot(r,r);
double v2 = dot(v,v);
double r_mag = norm(r);
double r3 = r2 * r_mag;
double p = GM/c2/r3;
// a
a.resize(3,0.0);
double pr = 2.0 * (beta + gama) * GM / r_mag - gama * v2;
double pv = 2.0 * (1.0 + gama) * dot(r,v);
a = p * ( pr * r + pv * v );
// da_dr
da_dr.resize(3,3,0.0);
double prr = -(GM/r_mag)*(GM/r_mag)*(2.0*(beta+gama)/c2);
double pvv = (GM/r3)*(2.0*(1.0+gama)/c2);
double par = -3.0/r2;
double ppr = (GM/r3)*((GM/r_mag)*(2.0*(beta+gama)/c2)-gama*v2/c2);
for(int i=0; i<3; i++)
{
for(int j=0; j<3; j++)
{
double det = (i == j) ? 1.0 : 0.0;
da_dr(i,j) = prr*r(i)*r(j)
+ pvv*v(i)*v(j)
+ par*a(i)*r(j)
+ ppr*det;
}
}
// da_dv
da_dv.resize(3,3,0.0);
double prv = -(GM/r3)*(2.0*gama/c2);
double pvr = (GM/r3)*(2.0*(1.0+gama)/c2);
double ppv = pvr*dot(r,v);
for(int i=0;i<3;i++)
{
for(int j=0;j<3;j++)
{
double det = (i == j) ? 1.0 : 0.0;
da_dr(i,j) = prv*r(i)*v(j)
+ pvr*v(i)*r(j)
+ ppv*det;
}
}
// da_dp add it later...
//da_GM da_dbeta da_gama
} // End of method 'RelativityEffect::doCompute()'
