//------------------------------------------------------------------------------
std::string SpiceAttitude::GetStringParameter(const Integer id,
const Integer index) const
{
if (id == ATTITUDE_KERNEL_NAME)
{
if ((index < 0) || (index >= (Integer) ck.size()))
{
std::string errmsg = "Error attempting to retrieve CK kernel name for object ";
errmsg += scName + " - index out-of-bounds.\n";
throw AttitudeException(errmsg);
}
return ck.at(index);
}
if (id == SC_CLOCK_KERNEL_NAME)
{
if ((index < 0) || (index >= (Integer) sclk.size()))
{
std::string errmsg = "Error attempting to retrieve SCLK kernel name for object ";
errmsg += scName + " - index out-of-bounds.\n";
throw AttitudeException(errmsg);
}
return sclk.at(index);
}
if (id == FRAME_KERNEL_NAME)
{
if ((index < 0) || (index >= (Integer) fk.size()))
{
std::string errmsg = "Error attempting to retrieve FK kernel name for object ";
errmsg += scName + " - index out-of-bounds.\n";
throw AttitudeException(errmsg);
}
return fk.at(index);
}
return Attitude::GetStringParameter(id, index);
}
//------------------------------------------------------------------------------
bool SpiceAttitude::SetStringParameter(const Integer id,
const std::string &value,
const Integer index)
{
#ifdef DEBUG_SPICE_ATTITUDE_GET_SET
MessageInterface::ShowMessage(
"Entering SetStringParameter with id = %d, value = \"%s\", index = %d\n",
id, value.c_str(), index);
#endif
if (id == ATTITUDE_KERNEL_NAME)
{
if ((index < 0) || (index > (Integer) ck.size()))
{
std::string errmsg = "Error attempting to set CK kernel name for object ";
errmsg += scName + " - index out-of-bounds.\n";
throw AttitudeException(errmsg);
}
if (index == (Integer) ck.size()) ck.push_back(value);
else ck.at(index) = value;
return true;
}
if (id == SC_CLOCK_KERNEL_NAME)
{
if ((index < 0) || (index > (Integer) sclk.size()))
{
std::string errmsg = "Error attempting to set SCLK kernel name for object ";
errmsg += scName + " - index out-of-bounds.\n";
throw AttitudeException(errmsg);
}
if (index == (Integer) sclk.size()) sclk.push_back(value);
else sclk.at(index) = value;
return true;
}
if (id == FRAME_KERNEL_NAME)
{
if ((index < 0) || (index > (Integer) fk.size()))
{
std::string errmsg = "Error attempting to set FK kernel name for object ";
errmsg += scName + " - index out-of-bounds.\n";
throw AttitudeException(errmsg);
}
if (index == (Integer) fk.size()) fk.push_back(value);
else fk.at(index) = value;
return true;
}
return Attitude::SetStringParameter(id, value, index);
}
//------------------------------------------------------------------------------
void SpiceAttitude::ComputeCosineMatrixAndAngularVelocity(Real atTime)
{
if (!isInitialized || needsReinit) Initialize();
#ifdef __USE_SPICE__
reader->GetTargetOrientation(scName, naifId, refFrameNaifId, atTime, dcm, angVel);
#else
std::string errmsg = "Error - attempting to use SpiceAttitude when ";
errmsg += "SPICE is not included in the GMAT build.\n";
throw AttitudeException(errmsg);
#endif
}
//---------------------------------------------------------------------------
bool CCSDSAttitude::Initialize()
{
#ifdef DEBUG_CCSDS_ATTITUDE
MessageInterface::ShowMessage("Entering CCSDSAttitude::Initialize\n");
#endif
bool isOK = Attitude::Initialize();
if (!isOK) return false;
// Changed to use aemFileFullPath (LOJ: 2014.06.26)
//if (aemFile == "")
if (aemFileFullPath == "")
{
std::string errmsg = "Error - AEM file name not set on CCSDS-AEM object.\n";
throw AttitudeException(errmsg);
}
//reader->SetFile(aemFile);
reader->SetFile(aemFileFullPath);
reader->Initialize();
return true;
}
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));
*/
}
//---------------------------------------------------------------------------
bool SpiceAttitude::Initialize()
{
#ifdef DEBUG_SPICE_ATTITUDE
MessageInterface::ShowMessage("Entering SpiceAttitude::Initialize\n");
#endif
bool isOK = Attitude::Initialize();
if (!isOK) return false;
if (scName == "")
{
std::string errmsg = "Error - object name not set on SpiceAttitude object.\n";
throw AttitudeException(errmsg);
}
if (ck.empty())
{
std::string errmsg = "Error - no CK pointing kernel(s) set on SpiceAttitude for object ";
errmsg += scName + "\n";
throw AttitudeException(errmsg);
}
if (sclk.empty())
{
std::string errmsg = "Error - no SCLK clock kernel(s) set on SpiceAttitude for object ";
errmsg += scName + "\n";
throw AttitudeException(errmsg);
}
if (fk.empty())
{
std::string warnmsg = "Warning - no FK frame kernel(s) set on SpiceAttitude for object ";
warnmsg += scName + ". A Frame Kernel may be necessary.\n";
MessageInterface::ShowMessage(warnmsg.c_str());
}
#ifdef __USE_SPICE__
// Load the CK kernel(s)
for (StringArray::iterator j = ck.begin(); j != ck.end(); ++j)
{
reader->LoadKernel(*j);
}
// Load the SCLK kernel(s)
for (StringArray::iterator j = sclk.begin(); j != sclk.end(); ++j)
{
reader->LoadKernel(*j);
}
// Load the FK kernel(s), if any
for (StringArray::iterator j = fk.begin(); j != fk.end(); ++j)
{
reader->LoadKernel(*j);
}
#endif
if (naifId == UNDEFINED_NAIF_ID)
{
#ifdef __USE_SPICE__
naifId = reader->GetNaifID(scName);
#ifdef DEBUG_SPICE_ATTITUDE
MessageInterface::ShowMessage(
"Retrieved NAIF ID for %s -> %d\n", scName.c_str(), naifId);
#endif
if (naifId == 0)
{
std::string errmsg = "Error - NAIF ID not available for object \n";
errmsg += scName + "\n";
throw AttitudeException(errmsg);
}
#else
std::string errmsg = "Error - NAIF ID not set on SpiceAttitude object.\n";
throw AttitudeException(errmsg);
#endif
}
if (refFrameNaifId == UNDEFINED_NAIF_ID_REF_FRAME)
{
std::string errmsg = "Error - NAIF ID for object reference frame not set on SpiceAttitude object.\n";
throw AttitudeException(errmsg);
}
return true;
}
