//------------------------------------------------------------------------------
bool RelativisticCorrection::Initialize()
{
PhysicalModel::Initialize();
if (!eop) throw ODEModelException(
wxT("EOP file is undefined for RelativisticCorrection ") + instanceName);
if (solarSystem != NULL)
{
body = solarSystem->GetBody(bodyName);
theSun = solarSystem->GetBody(SolarSystem::SUN_NAME);
if (body != NULL)
{
bodyMu = body->GetGravitationalConstant();
sunMu = theSun->GetGravitationalConstant();
#ifdef DEBUG_RELATIVISTIC_CORRECTION
MessageInterface::ShowMessage
(wxT("RelativisticCorrection::Initialize() setting mu=%f for type=%s, ")
wxT("name=%s\n"), bodyMu, body->GetTypeName().c_str(),
body->GetName().c_str());
#endif
}
else
{
initialized = false;
throw ODEModelException(wxT("RelativisticCorrection::Initialize() body \"") +
bodyName + wxT("\" is not in the solar system\n"));
}
}
else
{
initialized = false;
throw ODEModelException(
wxT("RelativisticCorrection::Initialize() solarSystem is NULL\n"));
}
// Create the coordinate systems needed (Earth only)
if (body->GetName() == GmatSolarSystemDefaults::EARTH_NAME)
{
bodyInertial = CoordinateSystem::CreateLocalCoordinateSystem(wxT("bodyInertial"), wxT("MJ2000Eq"), body,
NULL, NULL, body->GetJ2000Body(), solarSystem);
bodyFixed = CoordinateSystem::CreateLocalCoordinateSystem(wxT("bodyFixed"), wxT("BodyFixed"), body,
NULL, NULL, body->GetJ2000Body(), solarSystem);
}
return true;
}
//------------------------------------------------------------------------------
bool RelativisticCorrection::GetDerivatives(Real *state, Real dt, Integer order, const Integer id)
{
if ((order > 2) || (order < 1))
return false;
if ((cartesianCount < 1) && (stmCount < 1) && (aMatrixCount < 1))
throw ODEModelException(
wxT("RelativisticCorrection requires at least one spacecraft."));
now = epoch + dt/GmatTimeConstants::SECS_PER_DAY;
if (fillCartesian)
{
Rvector6 stateWRTSun, dummy, dummyResult;
Real r, v, s1, posMag, rvDotvvX4, s2_1, lt1, lt2;
Real muSunc2r3;
Real threeOver2 = 3.0 / 2.0;
Rmatrix33 RB_IF;
Real ar[3], geodesic[3], s2[3], s3[3], schwarzschild[3], J1[3], J[3], rvCrossvv[3], vvCrossJ[3];
Real rv[3], vv[3], omega[3], vel[3], pos[3], lenseThirring[3], posWRTSun[3], velWRTSun[3];
Real c = GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM * GmatMathConstants::M_TO_KM;
J[0] = J[1] = J[2] = 0.0;
omega[0] = omega[1] = omega[2] = 0.0;
geodesic[0] = geodesic[1] = geodesic[2] = 0.0;
lenseThirring[0] = lenseThirring[1] = lenseThirring[2] = 0.0;
if (body->GetName() == GmatSolarSystemDefaults::EARTH_NAME)
{
// compute quantities needed for geodesic and lense-thirring terms, for Earth ONLY
stateWRTSun = - theSun->GetMJ2000State(now); // since state of Earth wrt Earth = (0.0 0.0 0.0 0.0 0.0 0.0)
posWRTSun[0] = stateWRTSun[0];
posWRTSun[1] = stateWRTSun[1];
posWRTSun[2] = stateWRTSun[2];
velWRTSun[0] = stateWRTSun[3];
velWRTSun[1] = stateWRTSun[4];
velWRTSun[2] = stateWRTSun[5];
posMag = GmatMathUtil::Sqrt(posWRTSun[0] * posWRTSun[0] + posWRTSun[1] * posWRTSun[1] + posWRTSun[2] * posWRTSun[2]);
muSunc2r3 = sunMu / (c * c * posMag * posMag * posMag);
vel[0] = threeOver2 * velWRTSun[0];
vel[1] = threeOver2 * velWRTSun[1];
vel[2] = threeOver2 * velWRTSun[2];
pos[0] = -muSunc2r3 * posWRTSun[0];
pos[1] = -muSunc2r3 * posWRTSun[1];
pos[2] = -muSunc2r3 * posWRTSun[2];
omega[0] = vel[1]*pos[2] - vel[2]*pos[1];
omega[1] = vel[2]*pos[0] - vel[0]*pos[2];
omega[2] = vel[0]*pos[1] - vel[1]*pos[0];
cc.Convert(now, dummy, bodyFixed, dummyResult, bodyInertial);
bodyRadius = body->GetEquatorialRadius();
// Get xp and yp from the EOP file
// Real xp, yp;
// Real lod = 0.0; // Set this to 0.0 for now
// Real utcmjd = TimeConverterUtil::Convert(now.Get(), TimeConverterUtil::A1MJD, TimeConverterUtil::UTCMJD,
// GmatTimeConstants::JD_JAN_5_1941);
// eop->GetPolarMotionAndLod(utcmjd, xp, yp, lod);
bodySpinRate = 7.29211514670698e-5; // * (1.0 - (lod / GmatTimeConstants::SECS_PER_DAY));
RB_IF = cc.GetLastRotationMatrix();
J1[0] = 0.0;
J1[1] = 0.0;
J1[2] = (2.0 / 5.0) * bodyRadius * bodyRadius * bodySpinRate;
J[0] = RB_IF(0,0)*J1[0] + RB_IF(0,1)*J1[1] + RB_IF(0,2)*J1[2];
J[1] = RB_IF(1,0)*J1[0] + RB_IF(1,1)*J1[1] + RB_IF(1,2)*J1[2];
J[2] = RB_IF(2,0)*J1[0] + RB_IF(2,1)*J1[1] + RB_IF(2,2)*J1[2];
#ifdef DEBUG_RELATIVISTIC_CORRECTION
MessageInterface::ShowMessage(wxT("posWRTSun = %12.10f %12.10f %12.10f\n"), posWRTSun[0], posWRTSun[1], posWRTSun[2]);
MessageInterface::ShowMessage(wxT("velWRTSun = %12.10f %12.10f %12.10f\n"), velWRTSun[0], velWRTSun[1], velWRTSun[2]);
MessageInterface::ShowMessage(wxT("big Omega = %12.10f %12.10f %12.10f\n"), omega[0], omega[1], omega[2]);
MessageInterface::ShowMessage(wxT("J = %12.10f %12.10f %12.10f\n"), J[0], J[1], J[2]);
MessageInterface::ShowMessage(wxT("c = %12.10f\n"), c);
MessageInterface::ShowMessage(wxT("sunMu = %12.10f\n"), sunMu);
MessageInterface::ShowMessage(wxT("bodyMu = %12.10f\n"), bodyMu);
#endif
}
Integer nOffset;
for (Integer n = 0; n < cartesianCount; ++n)
{
nOffset = cartesianStart + n * 6; // stateSize;
for (Integer i = 0; i < 3; ++i)
{
rv[i] = state[i+nOffset];
vv[i] = state[i+nOffset+3];
}
// Compute the Schwarzschild solution
r = GmatMathUtil::Sqrt(rv[0] * rv[0] + rv[1] * rv[1] + rv[2] * rv[2]);
v = GmatMathUtil::Sqrt(vv[0] * vv[0] + vv[1] * vv[1] + vv[2] * vv[2]);
s1 = bodyMu / (c * c * r * r * r);
s2_1 = (4.0 * bodyMu / r) - (v * v);
s2[0] = s2_1 * rv[0];
s2[1] = s2_1 * rv[1];
s2[2] = s2_1 * rv[2];
rvDotvvX4 = 4.0 * (rv[0] * vv[0] + rv[1] * vv[1] + rv[2] * vv[2]);
s3[0] = rvDotvvX4 * vv[0];
s3[1] = rvDotvvX4 * vv[1];
s3[2] = rvDotvvX4 * vv[2];
schwarzschild[0] = s1 * (s2[0] + s3[0]);
schwarzschild[1] = s1 * (s2[1] + s3[1]);
schwarzschild[2] = s1 * (s2[2] + s3[2]);
// ONLY IF the body is the Earth, compute the other terms
if (body->GetName() == GmatSolarSystemDefaults::EARTH_NAME)
{
// Compute the geodesic precession
geodesic[0] = 2.0 * (omega[1]*vv[2] - omega[2]*vv[1]);
geodesic[1] = 2.0 * (omega[2]*vv[0] - omega[0]*vv[2]);
geodesic[2] = 2.0 * (omega[0]*vv[1] - omega[1]*vv[0]);
// Compute the Lense-Thirring precession
// J = 11.8 * omega.GetUnitVector(); // ******************** TEMPORARY - Steve's value
rvCrossvv[0] = rv[1]*vv[2] - rv[2]*vv[1];
rvCrossvv[1] = rv[2]*vv[0] - rv[0]*vv[2];
rvCrossvv[2] = rv[0]*vv[1] - rv[1]*vv[0];
vvCrossJ[0] = vv[1]*J[2] - vv[2]*J[1];
vvCrossJ[1] = vv[2]*J[0] - vv[0]*J[2];
vvCrossJ[2] = vv[0]*J[1] - vv[1]*J[0];
lt1 = 2.0 * s1;
lt2 = (3.0 / (r * r)) * (rv[0] * J[0] + rv[1] * J[1] + rv[2] * J[2]);
lenseThirring[0] = lt1 * ((lt2 * rvCrossvv[0]) + vvCrossJ[0]);
lenseThirring[1] = lt1 * ((lt2 * rvCrossvv[1]) + vvCrossJ[1]);
lenseThirring[2] = lt1 * ((lt2 * rvCrossvv[2]) + vvCrossJ[2]);
//Add the terms together
ar[0] = schwarzschild[0] + geodesic[0] + lenseThirring[0];
ar[1] = schwarzschild[1] + geodesic[1] + lenseThirring[1];
ar[2] = schwarzschild[2] + geodesic[2] + lenseThirring[2];
}
else
{
ar[0] = schwarzschild[0];
ar[1] = schwarzschild[1];
ar[2] = schwarzschild[2];
}
#ifdef DEBUG_RELATIVISTIC_CORRECTION
MessageInterface::ShowMessage(wxT("schwarzschild = %12.10f %12.10f %12.10f\n"), schwarzschild[0], schwarzschild[1], schwarzschild[2]);
MessageInterface::ShowMessage(wxT("geodesic = %12.10f %12.10f %12.10f\n"), geodesic[0], geodesic[1], geodesic[2]);
MessageInterface::ShowMessage(wxT("lenseThirring = %12.10f %12.10f %12.10f\n"), lenseThirring[0], lenseThirring[1], lenseThirring[2]);
#endif
// Fill Derivatives
switch (order)
{
case 1:
deriv[0+nOffset] = 0.0;
deriv[1+nOffset] = 0.0;
deriv[2+nOffset] = 0.0;
deriv[3+nOffset] = ar[0];
deriv[4+nOffset] = ar[1];
deriv[5+nOffset] = ar[2];
break;
case 2:
deriv[0+nOffset] = ar[0];
deriv[1+nOffset] = ar[1];
deriv[2+nOffset] = ar[2];
deriv[3+nOffset] = 0.0;
deriv[4+nOffset] = 0.0;
deriv[5+nOffset] = 0.0;
break;
}
}
} // fillCartesian
if (fillSTM)
{
// Setting all zeroes for now
Real aTilde[36];
Integer element;
for (Integer i = 0; i < stmCount; ++i)
{
Integer i6 = stmStart + i * 36;
// Calculate A-tilde
aTilde[ 0] = aTilde[ 1] = aTilde[ 2] =
aTilde[ 3] = aTilde[ 4] = aTilde[ 5] =
aTilde[ 6] = aTilde[ 7] = aTilde[ 8] =
aTilde[ 9] = aTilde[10] = aTilde[11] =
aTilde[12] = aTilde[13] = aTilde[14] =
aTilde[15] = aTilde[16] = aTilde[17] =
aTilde[18] = aTilde[19] = aTilde[20] =
aTilde[21] = aTilde[22] = aTilde[23] =
aTilde[24] = aTilde[25] = aTilde[26] =
aTilde[27] = aTilde[28] = aTilde[29] =
aTilde[30] = aTilde[31] = aTilde[32] =
aTilde[33] = aTilde[34] = aTilde[35] = 0.0;
for (Integer j = 0; j < 6; j++)
{
for (Integer k = 0; k < 6; k++)
{
element = j * 6 + k;
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(wxT("------ deriv[%d] = %12.10f\n"), (i6+element), aTilde[element]);
#endif
deriv[i6+element] = aTilde[element];
}
}
}
}
if (fillAMatrix)
{
// Setting all zeroes for now
Real aTilde[36];
Integer element;
for (Integer i = 0; i < aMatrixCount; ++i)
{
Integer i6 = aMatrixStart + i * 36;
// Calculate A-tilde
aTilde[ 0] = aTilde[ 1] = aTilde[ 2] =
aTilde[ 3] = aTilde[ 4] = aTilde[ 5] =
aTilde[ 6] = aTilde[ 7] = aTilde[ 8] =
aTilde[ 9] = aTilde[10] = aTilde[11] =
aTilde[12] = aTilde[13] = aTilde[14] =
aTilde[15] = aTilde[16] = aTilde[17] =
aTilde[18] = aTilde[19] = aTilde[20] =
aTilde[21] = aTilde[22] = aTilde[23] =
aTilde[24] = aTilde[25] = aTilde[26] =
aTilde[27] = aTilde[28] = aTilde[29] =
aTilde[30] = aTilde[31] = aTilde[32] =
aTilde[33] = aTilde[34] = aTilde[35] = 0.0;
for (Integer j = 0; j < 6; j++)
{
for (Integer k = 0; k < 6; k++)
{
element = j * 6 + k;
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(wxT("------ deriv[%d] = %12.10f\n"), (i6+element), aTilde[element]);
#endif
deriv[i6+element] = aTilde[element];
}
}
}
}
return true;
}
//------------------------------------------------------------------------------
bool PointMassForce::Initialize()
{
//MessageInterface::ShowMessage(wxT("PointMassForce::Initialize() entered\n"));
PhysicalModel::Initialize();
if (solarSystem != NULL)
{
//MessageInterface::ShowMessage(
// wxT("PointMassForce::Initialize() bodyName=%s\n"), bodyName.c_str());
body = solarSystem->GetBody(bodyName); //loj: 5/7/04 added
if (body != NULL)
{
mu = body->GetGravitationalConstant();
#if DEBUG_PMF_BODY
MessageInterface::ShowMessage
(wxT("PointMassForce::Initialize() setting mu=%f for type=%s, ")
wxT("name=%s\n"), mu, body->GetTypeName().c_str(),
body->GetName().c_str());
#endif
#ifdef DEBUG_FORCE_MODEL
MessageInterface::ShowMessage(
wxT("%s%s%s%16le\n"),
wxT("Point mass body "), body->GetName().c_str(),
wxT(" has mu = "), mu);
#endif
}
else
{
MessageInterface::ShowMessage(
wxT("PointMassForce::Initialize() body \"%s\" is not in the solar ")
wxT("system\n"), bodyName.c_str());
initialized = false;
throw ODEModelException(wxT("PointMassForce::Initialize() body \"") +
bodyName + wxT("\" is not in the solar system\n"));
}
}
else
{
MessageInterface::ShowMessage(
wxT("PointMassForce::Initialize() solarSystem is NULL\n"));
initialized = false;
throw ODEModelException(
wxT("PointMassForce::Initialize() solarSystem is NULL\n"));
}
Integer i6;
for (Integer i = 0; i < satCount; i++)
{
i6 = cartesianStart + i*6;
modelState[i6] = 7000.0 + 200.0 * i;
modelState[i6+1] = 300.0 * i;
modelState[i6+2] = 1000.0 - 100.0 * i;
modelState[i6+3] = 0.0;
modelState[i6+4] = 8.0 + 0.1 * i; // 7.61 km/s makes first one circular
modelState[i6+5] = 0.0;
}
return true;
}
