//------------------------------------------------------------------------------ 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; }