//------------------------------------------------------------------------------
// Returns the attitude (DCM) state from inertial-to-body at the specified
// input time.
//------------------------------------------------------------------------------
Rmatrix33 CCSDSAEMEulerAngleSegment::GetState(Real atEpoch)
{
#ifdef DEBUG_AEM_EULER_GET_STATE
MessageInterface::ShowMessage("====== Entering EulerSegment::GetState with epoch = %lf, dataStore size = %d\n",
atEpoch, (Integer) dataStore.size());
#endif
// DetermineState will look for an exact match; if so,
// return the state at that time; if not, then return the last
// state (if interpolation degree = 0) or else, interpolate
// to the requested time
Rvector eulerAngles(3);
eulerAngles = DetermineState(atEpoch);
#ifdef DEBUG_AEM_EULER_GET_STATE
MessageInterface::ShowMessage(" eulerAngles (deg) from DetermineState are: %12.10f %12.10f %12.10f\n",
eulerAngles[0] * GmatMathConstants::DEG_PER_RAD,
eulerAngles[1] * GmatMathConstants::DEG_PER_RAD,
eulerAngles[2] * GmatMathConstants::DEG_PER_RAD);
MessageInterface::ShowMessage(" and euler sequence is: %d %d %d\n", euler1, euler2, euler3);
#endif
// Conversion method has to have an Rvector3
Rvector3 theEulerAngles(eulerAngles(0), eulerAngles(1), eulerAngles(2));
Rmatrix33 theDCM = AttitudeConversionUtility::ToCosineMatrix(
theEulerAngles, euler1, euler2, euler3);
#ifdef DEBUG_AEM_EULER_GET_STATE
MessageInterface::ShowMessage("About to Exit EulerSegment::GetState, DCM = %s\n",
theDCM.ToString().c_str());
#endif
if (inertialToBody) return theDCM;
else return theDCM.Transpose();
}
void CoordinateConverter::RotationMatrixFromICRFToFK5(const A1Mjd &atEpoch)
{
Real theEpoch = atEpoch.Get();
#ifdef DEBUG_ICRF_TOFK5
MessageInterface::ShowMessage(
"Enter CoordinateConverter::RotationMatrixFromICRFToFK5 at epoch %18.12lf; \n\n", theEpoch);
#endif
// Specify Euler rotation vector for theEpoch:
Real vec[3];
ICRFFile* icrfFile = ICRFFile::Instance();
icrfFile->Initialize();
icrfFile->GetICRFRotationVector(theEpoch, &vec[0], 3, 9);
// Calculate rotation matrix based on Euler rotation vector:
Real angle = GmatMathUtil::Sqrt(vec[0]*vec[0]+vec[1]*vec[1]+vec[2]*vec[2]);
Real a[3];
a[0] = vec[0]/angle; a[1] = vec[1]/angle; a[2] = vec[2]/angle;
Real c = GmatMathUtil::Cos(angle);
Real s = GmatMathUtil::Sin(angle);
// rotation matrix from FK5 to ICRF:
Rmatrix33 rotM;
rotM.SetElement(0,0, c+a[0]*a[0]*(1-c));
rotM.SetElement(0,1, a[0]*a[1]*(1-c)+a[2]*s);
rotM.SetElement(0,2, a[0]*a[2]*(1-c)-a[1]*s);
rotM.SetElement(1,0, a[0]*a[1]*(1-c)-a[2]*s);
rotM.SetElement(1,1, c+a[1]*a[1]*(1-c));
rotM.SetElement(1,2, a[1]*a[2]*(1-c)+a[0]*s);
rotM.SetElement(2,0, a[0]*a[2]*(1-c)+a[1]*s);
rotM.SetElement(2,1, a[1]*a[2]*(1-c)-a[0]*s);
rotM.SetElement(2,2, c+a[2]*a[2]*(1-c));
// rotation matrix from ICRF to FK5:
icrfToFK5 = rotM.Transpose();
// rotation dot matrix from ICRF to FK5:
icrfToFK5Dot.Set(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
#ifdef DEBUG_ICRF_TOFK5
MessageInterface::ShowMessage("theEpoch = %18.12lf\n",theEpoch);
MessageInterface::ShowMessage("rotation vector = %18.12e %18.12e %18.12e\n", vec[0], vec[1], vec[2]);
MessageInterface::ShowMessage("R(0,0)=%18.12e, R(0,1)=%18.12e, R(0,2)=%18.12e\n",icrfToFK5(0,0),icrfToFK5(0,1),icrfToFK5(0,2));
MessageInterface::ShowMessage("R(1,0)=%18.12e, R(1,1)=%18.12e, R(1,2)=%18.12e\n",icrfToFK5(1,0),icrfToFK5(1,1),icrfToFK5(1,2));
MessageInterface::ShowMessage("R(2,0)=%18.12e, R(2,1)=%18.12e, R(2,2)=%18.12e\n",icrfToFK5(2,0),icrfToFK5(2,1),icrfToFK5(2,2));
MessageInterface::ShowMessage("Rdot(0,0)=%18.12e, Rdot(0,1)=%18.12e, Rdot(0,2)=%18.12e\n",icrfToFK5Dot(0,0),icrfToFK5Dot(0,1),icrfToFK5Dot(0,2));
MessageInterface::ShowMessage("Rdot(1,0)=%18.12e, Rdot(1,1)=%18.12e, Rdot(1,2)=%18.12e\n",icrfToFK5Dot(1,0),icrfToFK5Dot(1,1),icrfToFK5Dot(1,2));
MessageInterface::ShowMessage("Rdot(2,0)=%18.12e, Rdot(2,1)=%18.12e, Rdot(2,2)=%18.12e\n\n\n",icrfToFK5Dot(2,0),icrfToFK5Dot(2,1),icrfToFK5Dot(2,2));
#endif
#ifdef DEBUG_ICRF_TOFK5
MessageInterface::ShowMessage("NOW exiting CoordinateConverter::RotationMatrixFromICRFToFK5 ...\n\n");
#endif
}
//------------------------------------------------------------------------------
// void ConvertDeltaVToInertial(Real *dv, Real *dvInertial, Real epoch)
//------------------------------------------------------------------------------
void Burn::ConvertDeltaVToInertial(Real *dv, Real *dvInertial, Real epoch)
{
#ifdef DEBUG_BURN_CONVERT
MessageInterface::ShowMessage
("Burn::ConvertDeltaVToInertial(), usingLocalCoordSys=%d, coordSystemName='%s', "
"coordSystem=<%p>'%s'\n", usingLocalCoordSys, coordSystemName.c_str(),
coordSystem, coordSystem ? coordSystem->GetName().c_str() : "NULL");
#endif
if (usingLocalCoordSys && localCoordSystem == NULL)
{
throw BurnException
("Unable to convert burn elements to Inertial, the local Coordinate "
"System has not been created");
}
else if (!usingLocalCoordSys && coordSystem == NULL)
{
throw BurnException
("Unable to convert burn elements to Inertial, the Coordinate "
"System has not been set");
}
Real inDeltaV[6], outDeltaV[6];
for (Integer i=0; i<3; i++)
inDeltaV[i] = dv[i];
for (Integer i=3; i<6; i++)
inDeltaV[i] = 0.0;
// if not using local CS, use ref CoordinateSystem
if (!usingLocalCoordSys)
{
// Now rotate to MJ2000Eq axes, we don't want to translate so
// set coincident to true
coordSystem->ToBaseSystem(epoch, inDeltaV, outDeltaV, true); // @todo - need ToMJ2000Eq here?
#ifdef DEBUG_BURN_CONVERT_ROTMAT
Rmatrix33 rotMat = coordSystem->GetLastRotationMatrix();
MessageInterface::ShowMessage
("rotMat=\n%s\n", rotMat.ToString(16, 20).c_str());
#endif
dvInertial[0] = outDeltaV[0];
dvInertial[1] = outDeltaV[1];
dvInertial[2] = outDeltaV[2];
}
else
{
// if MJ2000Eq axes rotation matrix is always identity matrix
if (isMJ2000EqAxes)
{
dvInertial[0] = dv[0];
dvInertial[1] = dv[1];
dvInertial[2] = dv[2];
}
else if (isSpacecraftBodyAxes)
{
Rvector3 inDeltaV(dv[0], dv[1], dv[2]);
Rvector3 outDeltaV;
// Get attitude matrix from Spacecraft and transpose since
// attitude matrix from spacecraft gives rotation matrix from
// inertial to body
Rmatrix33 inertialToBody = spacecraft->GetAttitude(epoch);
Rmatrix33 rotMat = inertialToBody.Transpose();
#ifdef DEBUG_BURN_CONVERT_ROTMAT
MessageInterface::ShowMessage
("for local Spacecraft body ----- rotMat=\n%s\n", rotMat.ToString(16, 20).c_str());
#endif
outDeltaV = inDeltaV * rotMat;
for (Integer i=0; i<3; i++)
dvInertial[i] = outDeltaV[i];
}
else
{
// // Now rotate to MJ2000Eq axes
// localCoordSystem->ToMJ2000Eq(epoch, inDeltaV, outDeltaV, true);
// Now rotate to base system axes
localCoordSystem->ToBaseSystem(epoch, inDeltaV, outDeltaV, true); // @todo - need ToMJ2000Eq here?
dvInertial[0] = outDeltaV[0];
dvInertial[1] = outDeltaV[1];
dvInertial[2] = outDeltaV[2];
}
}
#ifdef DEBUG_BURN_CONVERT
MessageInterface::ShowMessage
("Burn::ConvertDeltaVToInertial() returning\n"
" dv = %f %f %f\n dvInertial = %f %f %f\n",
dv[0], dv[1], dv[2], dvInertial[0], dvInertial[1], dvInertial[2]);
#endif
}
//------------------------------------------------------------------------------
void GravityField::Calculate (Real dt, Real state[6],
Real acc[3], Rmatrix33& grad)
{
#ifdef DEBUG_CALCULATE
MessageInterface::ShowMessage(
"Entering Calculate with dt = %12.10f, state = %12.10f %12.10f %12.10f %12.10f %12.10f %12.10f\n",
dt, state[0], state[1], state[2], state[3], state[4], state[5]);
MessageInterface::ShowMessage(" acc = %12.10f %12.10f %12.10f\n", acc[0], acc[1], acc[2]);
#endif
Real jday = epoch + GmatTimeConstants::JD_JAN_5_1941 +
dt/GmatTimeConstants::SECS_PER_DAY;
// convert to body fixed coordinate system
Real now = epoch + dt/GmatTimeConstants::SECS_PER_DAY;
Real tmpState[6];
// CoordinateConverter cc; - move back to class, for performance
cc.Convert(now, state, inputCS, tmpState, fixedCS); // which CSs to use here???
#ifdef DEBUG_CALCULATE
MessageInterface::ShowMessage(
"After Convert, jday = %12.10f, now = %12.10f, and tmpState = %12.10f %12.10f %12.10f %12.10f %12.10f %12.10f\n",
jday, now, tmpState[0], tmpState[1], tmpState[2], tmpState[3], tmpState[4], tmpState[5]);
#endif
Rmatrix33 rotMatrix = cc.GetLastRotationMatrix();
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage("---->>>> rotMatrix = %s\n", rotMatrix.ToString().c_str());
#endif
// calculate sun and moon pos
Real sunpos[3] = {0.0,0.0,0.0};
Real moonpos[3] = {0.0,0.0,0.0};
Real sunMass = 0.0;
Real moonMass = 0.0;
// Acceleration
Real rotacc[3];
Rmatrix33 rotgrad;
bool useTides;
// for now, "None" and "SolidAndPole" are the only valid EarthTideModel values
if ((bodyName == GmatSolarSystemDefaults::EARTH_NAME) && (GmatStringUtil::ToUpper(earthTideModel) == "SOLIDANDPOLE"))
{
Real ep = epoch + dt / GmatTimeConstants::SECS_PER_DAY; // isn't this the same as now?
CelestialBody* theSun = solarSystem->GetBody(SolarSystem::SUN_NAME);
CelestialBody* theMoon = solarSystem->GetBody(SolarSystem::MOON_NAME);
if (!theSun || !theMoon)
throw ODEModelException("Solar system does not contain the Sun or Moon for Tide model.");
Rvector6 sunstateinertial = theSun->GetState(ep);
Rvector6 moonstateinertial = theMoon->GetState(ep);
Rvector6 sunstate;
Rvector6 moonstate;
cc.Convert(now, sunstateinertial, inputCS, sunstate, fixedCS);
cc.Convert(now, moonstateinertial, inputCS, moonstate, fixedCS);
sunstate.GetR(sunpos);
moonstate.GetR(moonpos);
sunMass = theSun->GetMass();
moonMass = theMoon->GetMass();
useTides = true;
}
else
useTides = false;
#ifdef DEBUG_GRAVITY_EARTH_TIDE
MessageInterface::ShowMessage("Calling gravityModel->CalculateFullField with useTides = %s\n",
(useTides? "true" : "false"));
#endif
// Get xp and yp from the EOP file
Real xp, yp, lod;
Real utcmjd = TimeConverterUtil::Convert(now, TimeConverterUtil::A1MJD, TimeConverterUtil::UTCMJD,
GmatTimeConstants::JD_JAN_5_1941);
eop->GetPolarMotionAndLod(utcmjd, xp, yp, lod);
bool computeMatrix = fillAMatrix || fillSTM;
gravityModel->CalculateFullField(jday, tmpState, degree, order, useTides, sunpos, moonpos, sunMass, moonMass,
xp, yp, computeMatrix, rotacc, rotgrad);
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage("after CalculateFullField, rotgrad = %s\n", rotgrad.ToString().c_str());
#endif
/*
MessageInterface::ShowMessage
("HarmonicField::Calculate pos= %20.14f %20.14f %20.14f\n",
tmpState[0],tmpState[1],tmpState[2]);
MessageInterface::ShowMessage
("HarmonicField::Calculate grad= %20.14e %20.14e %20.14e\n",
rotgrad(0,0),rotgrad(0,1),rotgrad(0,2));
MessageInterface::ShowMessage
("HarmonicField::Calculate grad= %20.14e %20.14e %20.14e\n",
rotgrad(1,0),rotgrad(1,1),rotgrad(1,2));
MessageInterface::ShowMessage
("HarmonicField::Calculate grad= %20.14e %20.14e %20.14e\n",
rotgrad(2,0),rotgrad(2,1),rotgrad(2,2));
*/
// Convert back to target CS
InverseRotate (rotMatrix,rotacc,acc);
grad = rotMatrix.Transpose() * rotgrad * rotMatrix;
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage("at end of Calculate, after rotation, grad = %s\n", grad.ToString().c_str());
#endif
}
