//------------------------------------------------------------------------------
// void ShowDerivative(const wxString &header, Real *state, Integer satCount)
//------------------------------------------------------------------------------
void PointMassForce::ShowDerivative(const wxString &header, Real *state,
Integer satCount)
{
#if DEBUG_PMF_DERV
static int debugCount2 = 0;
static bool showDeriv = true;
if (showDeriv)
{
MessageInterface::ShowMessage(wxT("%s\n"), header.c_str());
MessageInterface::ShowMessage(
wxT(">>>>>=======================================\n"));
Integer i6;
Rvector6 stateVec = Rvector6(state);
for (Integer i = 0; i < satCount; i++)
{
i6 = cartIndex + i * 6;
MessageInterface::ShowMessage
(wxT("sc#=%d state=%s\n"), i, stateVec.ToString().c_str());
MessageInterface::ShowMessage
(wxT("deriv=%f %f %f %f %f %f\n"), deriv[i6], deriv[i6+1],
deriv[i6+2], deriv[i6+3], deriv[i6+4], deriv[i6+5]);
}
MessageInterface::ShowMessage(
wxT("=======================================<<<<<\n"));
debugCount2++;
if (debugCount2 > 10)
showDeriv = false;
}
#endif
}
Rvector6 SpaceObject::GetLastState()
{
Rvector6 result;
Real *st = state.GetState();
result.Set(st[0], st[1], st[2], st[3], st[4], st[5]);
return result;
}
//---------------------------------------------------------------------------
const Rvector6 Barycenter::GetMJ2000State(const A1Mjd &atTime)
{
// if it's built-in, get the state from the SpacePoint
if (isBuiltIn)
{
lastStateTime = atTime;
lastState = builtInSP->GetMJ2000State(atTime);
#ifdef DEBUG_BARYCENTER_STATE
MessageInterface::ShowMessage("Computing state for Barycenter %s, whose builtInSP is %s\n",
instanceName.c_str(), (builtInSP->GetName()).c_str());
#endif
return lastState;
}
// otherwise, sum the masses and states
CheckBodies();
#ifdef DEBUG_BARYCENTER
MessageInterface::ShowMessage("Entering BaryCenter::GetMJ2000EqState at time %12.10f\n",
atTime.Get());
#endif
Real bodyMass = 0.0;
Rvector3 bodyPos(0.0,0.0,0.0);
Rvector3 bodyVel(0.0,0.0,0.0);
Real weight = 0.0;
Rvector3 sumMassPos(0.0,0.0,0.0);
Rvector3 sumMassVel(0.0,0.0,0.0);
Rvector6 bodyState;
Real sumMass = GetMass();
for (unsigned int i = 0; i < bodyList.size() ; i++)
{
bodyMass = ((CelestialBody*) (bodyList.at(i)))->GetMass();
bodyState = (bodyList.at(i))->GetMJ2000State(atTime);
bodyPos = bodyState.GetR();
bodyVel = bodyState.GetV();
weight = bodyMass/sumMass;
#ifdef DEBUG_BARYCENTER
MessageInterface::ShowMessage("Mass (and weight) of body %s = %12.10f (%12.10f)\n",
((bodyList.at(i))->GetName()).c_str(), bodyMass, weight);
MessageInterface::ShowMessage(" pos = %s\n", (bodyPos.ToString()).c_str());
MessageInterface::ShowMessage(" vel = %s\n", (bodyVel.ToString()).c_str());
#endif
sumMassPos += (weight * bodyPos);
sumMassVel += (weight * bodyVel);
}
#ifdef DEBUG_BARYCENTER
MessageInterface::ShowMessage("sumMassPos = %s\n",
(sumMassPos.ToString()).c_str());
#endif
lastState.Set(sumMassPos(0), sumMassPos(1), sumMassPos(2),
sumMassVel(0), sumMassVel(1), sumMassVel(2));
lastStateTime = atTime;
return lastState;
}
//------------------------------------------------------------------------------
Real EnvData::GetEnvReal(const std::string &str)
{
//-------------------------------------------------------
// 1. Get Spacecraft's central body (It's done in InitializeRefObjects())
// 2. Get current time (use parameter or sc pointer?)
// 3. Get spacecraft's position (use parameter?)
// 4. Call GetDensity() on central body
//-------------------------------------------------------
if (mSpacecraft == NULL || mSolarSystem == NULL || mOrigin == NULL)
InitializeRefObjects();
if (str == "AtmosDensity")
{
Real a1mjd = mSpacecraft->GetRealParameter("A1Epoch");
Rvector6 cartState = mSpacecraft->GetState().GetState();
// // Rvector6 cartState = mSpacecraft->GetStateVector("Cartesian");
// Rvector6 cartState = mSpacecraft->GetState(0);
// Real state[6];
// for (int i=0; i<6; i++)
// state[i] = cartState[i];
Real *state = (Real*)cartState.GetDataVector();
Real density = 0.0;
// Call GetDensity() on if origin is CelestialBody
if (mOrigin->IsOfType(Gmat::CELESTIAL_BODY))
{
if (((CelestialBody*)mOrigin)->GetDensity(state, &density, a1mjd, 1))
{
#ifdef DEBUG_ENVDATA_RUN
MessageInterface::ShowMessage
("EnvData::GetEnvReal() mOrigin=%s, a1mjd=%f, state=%s, density=%g\n",
mOrigin->GetName().c_str(), a1mjd, cartState.ToString().c_str(), density);
#endif
}
else
{
#ifdef DEBUG_ENVDATA_RUN
MessageInterface::ShowMessage
("EnvData::GetEnvReal() AtmosphereModel used for %s is NULL\n",
mOrigin->GetName().c_str());
#endif
}
}
return density;
}
else
{
throw ParameterException("EnvData::GetEnvReal() Unknown parameter name: " +
str);
}
}
//------------------------------------------------------------------------------
void GeocentricSolarEclipticAxes::CalculateRotationMatrix(const A1Mjd &atEpoch,
bool forceComputation)
{
Rvector6 rvSun = secondary->GetMJ2000State(atEpoch) -
primary->GetMJ2000State(atEpoch);
Rvector3 rSun = rvSun.GetR();
Rvector3 vSun = rvSun.GetV();
Real rMag = rSun.GetMagnitude();
Real vMag = vSun.GetMagnitude();
Rvector3 rUnit = rSun / rMag;
Rvector3 vUnit = vSun / vMag;
Rvector3 rxv = Cross(rSun,vSun);
Real rxvM = rxv.GetMagnitude();
Rvector3 x = rUnit;
Rvector3 z = rxv / rxvM;
Rvector3 y = Cross(z,x);
rotMatrix(0,0) = x(0);
rotMatrix(0,1) = y(0);
rotMatrix(0,2) = z(0);
rotMatrix(1,0) = x(1);
rotMatrix(1,1) = y(1);
rotMatrix(1,2) = z(1);
rotMatrix(2,0) = x(2);
rotMatrix(2,1) = y(2);
rotMatrix(2,2) = z(2);
Rvector3 vR = vSun / rMag;
Rvector3 xDot = vR - x * (x * vR);
Rvector3 zDot; // zero vector by default
Rvector3 yDot = Cross(z, xDot);
rotDotMatrix(0,0) = xDot(0);
rotDotMatrix(0,1) = yDot(0);
rotDotMatrix(0,2) = zDot(0);
rotDotMatrix(1,0) = xDot(1);
rotDotMatrix(1,1) = yDot(1);
rotDotMatrix(1,2) = zDot(1);
rotDotMatrix(2,0) = xDot(2);
rotDotMatrix(2,1) = yDot(2);
rotDotMatrix(2,2) = zDot(2);
}
int main(int argc, char **argv)
{
std::cout << "************************************************\n"
<< "*** StateType Conversion Unit Test Program\n"
<< "************************************************\n\n";
StateConverter stateConverter;
Rvector6 newState;
Real state[6];
state[0] = 7100;
state[1] = 0.0;
state[2] = 1300;
state[3] = 0.0;
state[4] = 7.35;
state[5] = 1.0;
std::cout << "\n--- Beginning with state ----";
std::cout << std::setprecision(8);
for (int i=0; i < 6; i++)
std::cout << "\n[" << i << "]: " << state[i];
std::cout << "\n--- Converting to Keplerian state ----";
newState = stateConverter.Convert(state,"Cartesian","Keplerian");
for (int i=0; i < 6; i++)
{
state[i] = newState.Get(i);
std::cout << "\n[" << i << "]: " << state[i];
}
std::cout << "\n--- Converting to Cartesian state ----";
newState = stateConverter.Convert(state,"Keplerian","Cartesian");
for (int i=0; i < 6; i++)
{
state[i] = newState.Get(i);
std::cout << "\n[" << i << "]: " << state[i];
}
std::cout << "\n********************* End of Testing ********************\n";
}
//------------------------------------------------------------------------------
const Rvector6 BodyFixedPoint::GetMJ2000State(const A1Mjd &atTime)
{
#ifdef DEBUG_BODYFIXED_STATE
MessageInterface::ShowMessage("In GetMJ2000State for BodyFixedPoint %s\n",
instanceName.c_str());
#endif
UpdateBodyFixedLocation();
Real epoch = atTime.Get();
Rvector6 bfState;
// For now I'm ignoring velocity; this assumes bfLocation is kept up-to-date
bfState.Set(bfLocation[0], bfLocation[1], bfLocation[2], 0.0, 0.0, 0.0);
// Convert from the body-fixed location to a J2000 location,
// assuming you have pointer to coordinate systems mj2k and bfcs,
// where mj2k is a J2000 system and bfcs is BodyFixed
#ifdef DEBUG_BODYFIXED_STATE
MessageInterface::ShowMessage("... before call to Convert, epoch = %12.10f\n",
epoch);
MessageInterface::ShowMessage(" ... bfcs (%s) = %s and mj2kcs (%s) = %s\n",
bfcsName.c_str(), (bfcs? "NOT NULL" : "NULL"),
mj2kcsName.c_str(), (mj2kcs? "NOT NULL" : "NULL"));
MessageInterface::ShowMessage("bf state (in bfcs, cartesian) = %s\n",
(bfState.ToString()).c_str());
MessageInterface::ShowMessage("SolarSystem is%s NULL\n", (solarSystem? "NOT " : ""));
#endif
ccvtr.Convert(epoch, bfState, bfcs, j2000PosVel, mj2kcs);
#ifdef DEBUG_BODYFIXED_STATE
MessageInterface::ShowMessage("bf state (in mj2kcs, cartesian) = %s\n",
(j2000PosVel.ToString()).c_str());
#endif
lastStateTime = atTime;
lastState = j2000PosVel;
return j2000PosVel;
}
//------------------------------------------------------------------------------
// void ShowBodyState(const wxString &header, Real time, Rvector6 &rv);
//------------------------------------------------------------------------------
void PointMassForce::ShowBodyState(const wxString &header, Real time,
Rvector6 &rv)
{
#if DEBUG_PMF_BODY
static int debugCount1 = 0;
static bool showBodyState = true;
if (showBodyState)
{
MessageInterface::ShowMessage(wxT("%s\n"), header.c_str());
MessageInterface::ShowMessage(
wxT(">>>>>=======================================\n"));
MessageInterface::ShowMessage(wxT("time=%f rv=%s\n"), time,
rv.ToString().c_str());
MessageInterface::ShowMessage(
wxT("=======================================<<<<<\n"));
debugCount1++;
if (debugCount1 > 10)
showBodyState = false;
}
#endif
}
void printState(const std::string &title, const Rvector6 state)
{
std::cout << "\n--------- " << title << " ---------\n";
for (int i=0; i < 6; i++)
std::cout << "[" << i << "]: " << state.Get(i) << std::endl;
}
//------------------------------------------------------------------------------
const Rvector3 SpaceObject::GetMJ2000Velocity(const A1Mjd &atTime)
{
const Rvector6 rv6 = GetMJ2000State(atTime);
return (rv6.GetV());
}
//------------------------------------------------------------------------------
const Rvector3 SpaceObject::GetMJ2000Position(const A1Mjd &atTime)
{
const Rvector6 rv6 = GetMJ2000State(atTime);
return (rv6.GetR());
}
//------------------------------------------------------------------------------
// static Rvector6 CartesianToKeplerian(const Rvector6 &cartVec, Real grav,
// Real *ma);
//------------------------------------------------------------------------------
Rvector6 CoordUtil::CartesianToKeplerian(const Rvector6 &cartVec, Real grav, Real *ma)
{
#ifdef DEBUG_CART_TO_KEPL
MessageInterface::ShowMessage(wxT("CoordUtil::CartesianToKeplerian called ... \n"));
#endif
Real kepl[6];
Real r[3];
Real v[3];
Real tfp;
Integer ret;
Integer errorCode;
for (int i=0; i<6; i++)
kepl[i] = 0.0;
if(grav < 1.0)
{
throw UtilityException(wxT("CoordUtil::CartesianToKeplerian() gravity constant ")
wxT("too small for conversion to Keplerian elements\n"));
}
else
{
cartVec.GetR(r);
cartVec.GetV(v);
//MessageInterface::ShowMessage(wxT("CoordUtil::CartesianToKeplerian() r=%f, %f, %f ")
// wxT("v=%f, %f, %f\n"), r[0], r[1], r[2], v[0], v[1], v[2]);
if (CoordUtil::IsRvValid(r,v))
{
errorCode = CoordUtil::ComputeCartToKepl(grav, r, v, &tfp, kepl, ma);
switch (errorCode)
{
case 0: // no error
ret = 1;
break;
case 2:
throw UtilityException
(wxT("CoordUtil::CartesianToKeplerian() ")
wxT("Gravity constant too small for conversion to Keplerian elements\n"));
default:
throw UtilityException
(wxT("CoordUtil::CartesianToKeplerian() ")
wxT("Unable to convert Cartesian elements to Keplerian\n"));
}
}
else
{
wxString ss;
//ss << cartVec;
throw UtilityException
(wxT("CoordUtil::CartesianToKeplerian() Invalid Cartesian elements:\n") +
ss);
}
}
Rvector6 keplVec = Rvector6(kepl[0], kepl[1], kepl[2], kepl[3], kepl[4], kepl[5]);
return keplVec;
}
//------------------------------------------------------------------------------
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
}
//---------------------------------------------------------------------------
const Rvector3 Barycenter::GetMJ2000Position(const A1Mjd &atTime)
{
Rvector6 tmp = GetMJ2000State(atTime);
return (tmp.GetR());
}
//------------------------------------------------------------------------------
const Rvector3 BodyFixedPoint::GetMJ2000Velocity(const A1Mjd &atTime)
{
Rvector6 rv = GetMJ2000State(atTime);
j2000Vel = rv.GetV();
return j2000Vel;
}
//------------------------------------------------------------------------------
const Rvector3 BodyFixedPoint::GetMJ2000Position(const A1Mjd &atTime)
{
Rvector6 rv = GetMJ2000State(atTime);
j2000Pos = rv.GetR();
return j2000Pos;
}
//---------------------------------------------------------------------------
void ObjectReferencedAxes::CalculateRotationMatrix(const A1Mjd &atEpoch,
bool forceComputation)
{
if (!primary)
throw CoordinateSystemException("Primary \"" + primaryName +
"\" is not yet set in object referenced coordinate system!");
if (!secondary)
throw CoordinateSystemException("Secondary \"" + secondaryName +
"\" is not yet set in object referenced coordinate system!");
if ((xAxis == yAxis) || (xAxis == zAxis) || (yAxis == zAxis))
{
CoordinateSystemException cse;
cse.SetDetails("For object referenced axes, axes are improperly "
"defined.\nXAxis = '%s', YAxis = '%s', ZAxis = '%s'",
xAxis.c_str(), yAxis.c_str(), zAxis.c_str());
throw cse;
}
if ((xAxis != "") && (yAxis != "") && (zAxis != ""))
{
CoordinateSystemException cse;
cse.SetDetails("For object referenced axes, too many axes are defined.\n"
"XAxis = '%s', YAxis = '%s', ZAxis = '%s'",
xAxis.c_str(), yAxis.c_str(), zAxis.c_str());
throw cse;
}
SpacePoint *useAsSecondary = secondary;
// if (!useAsSecondary) useAsSecondary = origin;
Rvector6 rv = useAsSecondary->GetMJ2000State(atEpoch) -
primary->GetMJ2000State(atEpoch);
#ifdef DEBUG_ROT_MATRIX
if (visitCount == 0)
{
MessageInterface::ShowMessage(" ------------ rv Primary (%s) to Secondary (%s) = %s\n",
primary->GetName().c_str(), secondary->GetName().c_str(), rv.ToString().c_str());
visitCount++;
}
#endif
#ifdef DEBUG_ROT_MATRIX
if (visitCount == 0)
{
std::stringstream ss;
ss.precision(30);
ss << " ----------------- rv Earth to Moon (truncated) = "
<< rv << std::endl;
MessageInterface::ShowMessage("%s\n", ss.str().c_str());
visitCount++;
}
#endif
Rvector3 a = useAsSecondary->GetMJ2000Acceleration(atEpoch) -
primary->GetMJ2000Acceleration(atEpoch);
Rvector3 r = rv.GetR();
Rvector3 v = rv.GetV();
Rvector3 n = Cross(r,v);
Rvector3 rUnit = r.GetUnitVector();
Rvector3 vUnit = v.GetUnitVector();
Rvector3 nUnit = n.GetUnitVector();
Real rMag = r.GetMagnitude();
Real vMag = v.GetMagnitude();
Real nMag = n.GetMagnitude();
// check for divide-by-zero
if ((GmatMathUtil::IsEqual(rMag, MAGNITUDE_TOL)) || (GmatMathUtil::IsEqual(vMag, MAGNITUDE_TOL)) || (GmatMathUtil::IsEqual(nMag, MAGNITUDE_TOL)))
{
std::string errmsg = "Object referenced axis system named \"";
errmsg += coordName + "\" is undefined because at least one axis is near zero in length.\n";
throw CoordinateSystemException(errmsg);
}
Rvector3 rDot = (v / rMag) - (rUnit / rMag) * (rUnit * v);
Rvector3 vDot = (a / vMag) - (vUnit / vMag) * (vUnit * a);
Rvector3 nDot = (Cross(r,a) / nMag) - (nUnit / nMag) * (Cross(r,a) * nUnit);
Rvector3 xUnit, yUnit, zUnit, xDot, yDot, zDot;
bool xUsed = true, yUsed = true, zUsed = true;
// determine the x-axis
if ((xAxis == "R") || (xAxis == "r"))
{
xUnit = rUnit;
xDot = rDot;
}
else if ((xAxis == "-R") || (xAxis == "-r"))
{
xUnit = -rUnit;
xDot = -rDot;
}
else if ((xAxis == "V") || (xAxis == "v"))
{
xUnit = vUnit;
xDot = vDot;
}
else if ((xAxis == "-V") || (xAxis == "-v"))
{
xUnit = -vUnit;
xDot = -vDot;
}
else if ((xAxis == "N") || (xAxis == "n"))
{
xUnit = nUnit;
xDot = nDot;
}
else if ((xAxis == "-N") || (xAxis == "-n"))
{
xUnit = -nUnit;
xDot = -nDot;
}
else
{
xUsed = false;
}
// determine the y-axis
if ((yAxis == "R") || (yAxis == "r"))
{
yUnit = rUnit;
yDot = rDot;
}
else if ((yAxis == "-R") || (yAxis == "-r"))
{
yUnit = -rUnit;
yDot = -rDot;
}
else if ((yAxis == "V") || (yAxis == "v"))
{
yUnit = vUnit;
yDot = vDot;
}
else if ((yAxis == "-V") || (yAxis == "-v"))
{
yUnit = -vUnit;
yDot = -vDot;
}
else if ((yAxis == "N") || (yAxis == "n"))
{
yUnit = nUnit;
yDot = nDot;
}
else if ((yAxis == "-N") || (yAxis == "-n"))
{
yUnit = -nUnit;
yDot = -nDot;
}
else
{
yUsed = false;
}
// determine the z-axis
if ((zAxis == "R") || (zAxis == "r"))
{
zUnit = rUnit;
zDot = rDot;
}
else if ((zAxis == "-R") || (zAxis == "-r"))
{
zUnit = -rUnit;
zDot = -rDot;
}
else if ((zAxis == "V") || (zAxis == "v"))
{
zUnit = vUnit;
zDot = vDot;
}
else if ((zAxis == "-V") || (zAxis == "-v"))
{
zUnit = -vUnit;
zDot = -vDot;
}
else if ((zAxis == "N") || (zAxis == "n"))
{
zUnit = nUnit;
zDot = nDot;
}
else if ((zAxis == "-N") || (zAxis == "-n"))
{
zUnit = -nUnit;
zDot = -nDot;
}
else
{
zUsed = false;
}
// determine the third axis
if (xUsed && yUsed && !zUsed)
{
zUnit = Cross(xUnit, yUnit);
zDot = Cross(xDot, yUnit) + Cross(xUnit, yDot);
}
else if (xUsed && zUsed && !yUsed)
{
yUnit = Cross(zUnit,xUnit);
yDot = Cross(zDot, xUnit) + Cross(zUnit, xDot);
}
else if (yUsed && zUsed && !xUsed)
{
xUnit = Cross(yUnit,zUnit);
xDot = Cross(yDot, zUnit) + Cross(yUnit, zDot);
}
else
{
throw CoordinateSystemException(
"Object referenced axes are improperly defined.");
}
// Compute the rotation matrix
rotMatrix(0,0) = xUnit(0);
rotMatrix(0,1) = yUnit(0);
rotMatrix(0,2) = zUnit(0);
rotMatrix(1,0) = xUnit(1);
rotMatrix(1,1) = yUnit(1);
rotMatrix(1,2) = zUnit(1);
rotMatrix(2,0) = xUnit(2);
rotMatrix(2,1) = yUnit(2);
rotMatrix(2,2) = zUnit(2);
// Compute the rotation derivative matrix
rotDotMatrix(0,0) = xDot(0);
rotDotMatrix(0,1) = yDot(0);
rotDotMatrix(0,2) = zDot(0);
rotDotMatrix(1,0) = xDot(1);
rotDotMatrix(1,1) = yDot(1);
rotDotMatrix(1,2) = zDot(1);
rotDotMatrix(2,0) = xDot(2);
rotDotMatrix(2,1) = yDot(2);
rotDotMatrix(2,2) = zDot(2);
#ifdef DEBUG_ROT_MATRIX
MessageInterface::ShowMessage
("rotMatrix=%s\n", rotMatrix.ToString().c_str());
std::stringstream ss;
ss.setf(std::ios::fixed);
ss.precision(30);
ss << " ----------------- rotMatrix = " << rotMatrix << std::endl;
ss.setf(std::ios::scientific);
ss << " ----------------- rotDotMatrix = " << rotDotMatrix << std::endl;
MessageInterface::ShowMessage("%s\n", ss.str().c_str());
#endif
if (!rotMatrix.IsOrthonormal(ORTHONORMAL_TOL))
{
std::stringstream errmsg("");
errmsg << "*** WARNING*** Object referenced axis system \"" << coordName;
errmsg << "\" has a non-orthogonal rotation matrix. " << std::endl;
}
}
//---------------------------------------------------------------------------
const Rvector3 Barycenter::GetMJ2000Velocity(const A1Mjd &atTime)
{
Rvector6 tmp = GetMJ2000State(atTime);
return (tmp.GetV());
}
void NadirPointing::ComputeCosineMatrixAndAngularVelocity(Real atTime)
{
//#ifdef DEBUG_NADIRPOINTING
//#endif
#ifdef DEBUG_NADIR_POINTING_COMPUTE
MessageInterface::ShowMessage("In NadirPointing, computing at time %le\n",
atTime);
MessageInterface::ShowMessage("ModeOfConstraint = %s\n", modeOfConstraint.c_str());
if (!owningSC) MessageInterface::ShowMessage("--- owningSC is NULL!!!!\n");
if (!refBody) MessageInterface::ShowMessage("--- refBody is NULL!!!!\n");
#endif
if (!isInitialized || needsReinit)
{
Initialize();
// Check for unset reference body pointer
if (!refBody)
{
std::string attEx = "Reference body ";
attEx += refBodyName + " not defined for attitude of type \"";
attEx += "NadirPointing\"";
throw AttitudeException(attEx);
}
}
/// using a spacecraft object, *owningSC
A1Mjd theTime(atTime);
Rvector6 scState = ((SpaceObject*) owningSC)->GetMJ2000State(theTime);
Rvector6 refState = refBody->GetMJ2000State(theTime);
Rvector3 pos(scState[0] - refState[0], scState[1] - refState[1], scState[2] - refState[2]);
Rvector3 vel(scState[3] - refState[3], scState[4] - refState[4], scState[5] - refState[5]);
#ifdef DEBUG_NADIR_POINTING_COMPUTE
MessageInterface::ShowMessage(" scState = %s\n", scState.ToString().c_str());
MessageInterface::ShowMessage(" refState = %s\n", refState.ToString().c_str());
MessageInterface::ShowMessage(" pos = %s\n", pos.ToString().c_str());
MessageInterface::ShowMessage(" vel = %s\n", vel.ToString().c_str());
#endif
// Error message
if ( bodyAlignmentVector.GetMagnitude() < 1.0e-5 )
{
throw AttitudeException("The size of BodyAlignmentVector is too small.");
}
else if ( bodyConstraintVector.GetMagnitude() < 1.0e-5 )
{
throw AttitudeException("The size of BodyConstraintVector is too small.");
}
else if ( bodyAlignmentVector.GetUnitVector()*bodyConstraintVector.GetUnitVector() > ( 1.0 - 1.0e-5) )
{
throw AttitudeException("BodyAlignmentVector and BodyConstraintVector are the same.");
}
else if ( pos.GetMagnitude() < 1.0e-5 )
{
throw AttitudeException("A position vector is zero.");
}
else if ( vel.GetMagnitude() < 1.0e-5 )
{
throw AttitudeException("A velocity vector is zero.");
}
else if ( Cross(pos,vel).GetMagnitude() < 1.0e-5 )
{
throw AttitudeException("Cross product of position and velocity is zero: LVLH frame cannot be defined.");
}
Rvector3 normal = Cross(pos,vel);
Rvector3 xhat = pos/pos.GetMagnitude();
Rvector3 yhat = normal/normal.GetMagnitude();
Rvector3 zhat = Cross(xhat,yhat);
#ifdef DEBUG_NADIR_POINTING_COMPUTE
MessageInterface::ShowMessage(" normal = %s\n", normal.ToString().c_str());
MessageInterface::ShowMessage(" xhat = %s\n", xhat.ToString().c_str());
MessageInterface::ShowMessage(" yhat = %s\n", yhat.ToString().c_str());
MessageInterface::ShowMessage(" zhat = %s\n", zhat.ToString().c_str());
#endif
// RiI calculation (from inertial to LVLH)
Rmatrix33 RIi;
RIi(0,0) = xhat(0);
RIi(1,0) = xhat(1);
RIi(2,0) = xhat(2);
RIi(0,1) = yhat(0);
RIi(1,1) = yhat(1);
RIi(2,1) = yhat(2);
RIi(0,2) = zhat(0);
RIi(1,2) = zhat(1);
RIi(2,2) = zhat(2);
// Set alignment/constraint vector in body frame
if ( modeOfConstraint == "OrbitNormal" )
{
referenceVector[0] = -1;
referenceVector[1] = 0;
referenceVector[2] = 0;
constraintVector[0] = 0;
constraintVector[1] = 1;
constraintVector[2] = 0;
}
else if ( modeOfConstraint == "VelocityConstraint" )
{
referenceVector[0] = -1;
referenceVector[1] = 0;
referenceVector[2] = 0;
constraintVector[0] = 0;
constraintVector[1] = 0;
constraintVector[2] = 1;
}
// RBi calculation using TRIAD (from LVLH to body frame)
Rmatrix33 RiB = TRIAD( bodyAlignmentVector, bodyConstraintVector, referenceVector, constraintVector );
// the rotation matrix (from body to inertial)
dcm = RIi*RiB;
// the final rotation matrix (from inertial to body)
dcm = dcm.Transpose();
// We do NOT calculate angular velocity for Nadir.
// TODO : Throw AttitudeException??
/*
Rmatrix33 wxIBB = - RBi*RiIDot*dcm.Transpose();
Rvector3 wIBB;
wIBB.Set(wxIBB(2,1),wxIBB(0,2),wxIBB(1,0));
*/
}
//------------------------------------------------------------------------------
Rvector6 PointMassForce::GetDerivativesForSpacecraft(Spacecraft *sc)
{
Rvector6 dv;
Real *j2kState = sc->GetState().GetState();
Real state[6];
Real now = sc->GetEpoch();
BuildModelState(now, state, j2kState);
#ifdef DEBUG_FIRST_CALL
if (!firstCallFired)
MessageInterface::ShowMessage("Point Mass %s: Pos = [%lf %lf %lf] -> ",
bodyName.c_str(), state[0], state[1], state[2]);
#endif
Real radius, r3, mu_r, rbb3, mu_rbb, a_indirect[3];
Real relativePosition[3];
Rvector6 bodyrv = body->GetState(now);
Rvector6 orig = forceOrigin->GetMJ2000State(now);
const Real *brv = bodyrv.GetDataVector(), *orv = orig.GetDataVector();
Real rv[3];
rv[0] = brv[0] - orv[0];
rv[1] = brv[1] - orv[1];
rv[2] = brv[2] - orv[2];
// The vector from the force origin to the gravitating body
// Precalculations for the indirect effect term
rbb3 = rv[0]*rv[0]+rv[1]*rv[1]+rv[2]*rv[2];
if (rbb3 != 0.0)
{
//rbb3 *= sqrt(rbb3);
rbb3 = sqrt(rbb3 * rbb3 * rbb3);
mu_rbb = mu / rbb3;
a_indirect[0] = mu_rbb * rv[0];
a_indirect[1] = mu_rbb * rv[1];
a_indirect[2] = mu_rbb * rv[2];
}
else
a_indirect[0] = a_indirect[1] = a_indirect[2] = 0.0;
{
//relativePosition[0] = rv[0] - state[0];
//relativePosition[1] = rv[1] - state[1];
//relativePosition[2] = rv[2] - state[2];
relativePosition[0] = - state[0];
relativePosition[1] = - state[1];
relativePosition[2] = - state[2];
r3 = relativePosition[0]*relativePosition[0] +
relativePosition[1]*relativePosition[1] +
relativePosition[2]*relativePosition[2];
radius = sqrt(r3);
r3 *= radius;
mu_r = mu / r3;
dv[3] = relativePosition[0] * mu_r - a_indirect[0];
dv[4] = relativePosition[1] * mu_r - a_indirect[1];
dv[5] = relativePosition[2] * mu_r - a_indirect[2];
#ifdef DEBUG_DERIVATIVES_FOR_SPACECRAFT
MessageInterface::ShowMessage("***PMF AX in GD4SC: %.12lf * %.12le -"
" %.12le = %.12le\n", relativePosition[0], mu_r, a_indirect[0],
dv[3]);
#endif
}
#ifdef DEBUG_FIRST_CALL
if (!firstCallFired)
MessageInterface::ShowMessage("Accel = [%le %le %le]\n", dv[3],
dv[4], dv[5]);
#endif
return dv;
}
