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