//------------------------------------------------------------------------------
// Adds an epoch/data pair to the dataStore.
// At this level, it just checks for errors in data size or epoch order.
//------------------------------------------------------------------------------
bool CCSDSEMSegment::AddData(Real epoch, Rvector data)
{
if (data.GetSize() != dataSize)
{
std::ostringstream errmsg("");
errmsg << segError;
errmsg << "Data of type " << dataType;
errmsg << " must contain " << dataSize;
errmsg << " elements.\n";
throw UtilityException(errmsg.str());
}
if ((epoch < startTime) || (epoch > stopTime))
{
std::ostringstream errmsg("");
errmsg.precision(16);
errmsg << segError;
errmsg << "Data epoch " << epoch << " must be between the start time, ";
errmsg << startTime << ", and the stop time, " << stopTime;
errmsg << ", for the segment.\n";
throw UtilityException(errmsg.str());
}
if (!dataStore.empty())
{
Real lastEpoch = dataStore.at(dataStore.size()-1)->epoch;
if (epoch <= lastEpoch)
{
std::string errmsg = segError;
errmsg += "Epochs within DATA segment must be increasing.\n";
throw UtilityException(errmsg);
}
}
return true;
}
//------------------------------------------------------------------------------
// Adds an epoch/data pair to the dataStore.
//------------------------------------------------------------------------------
bool CCSDSAEMEulerAngleSegment::AddData(Real epoch, Rvector data)
{
// First, check for data size and ordering
CCSDSAEMSegment::AddData(epoch, data);
// We need to store the angles in radians
Rvector useData(3, data[0] * GmatMathConstants::RAD_PER_DEG,
data[1] * GmatMathConstants::RAD_PER_DEG,
data[2] * GmatMathConstants::RAD_PER_DEG);
if (!ValidateEulerAngles(useData))
{
std::string errmsg = segError;
errmsg += "Data within DATA segment are not valid Euler Angles.\n";
throw UtilityException(errmsg);
}
EpochAndData *newData = new EpochAndData();
newData->epoch = epoch;
newData->data = useData;
dataStore.push_back(newData);
#ifdef DEBUG_AEM_EULER_DATA
MessageInterface::ShowMessage("----> For (Euler Angle) segment number %d, added epoch = "
"%12.10f and data = %12.10f %12.10f %12.10f\n",
segmentNumber, epoch, useData[0], useData[1], useData[2]);
#endif
return true;
}
UtilityException UtilityException::inherit(
UTIL_EXCEPTION_CONSTRUCTOR_ARGS_LIST) throw() {
Exception cause;
Exception::NamedErrorCode modErrorCode;
try {
throw;
}
catch (Exception &e) {
modErrorCode = e.getNamedErrorCode();
cause = e;
}
catch (std::bad_alloc&) {
modErrorCode = UTIL_EXCEPTION_UTIL_NAMED_CODE(CODE_NO_MEMORY);
}
catch (...) {
modErrorCode = UTIL_EXCEPTION_UTIL_NAMED_CODE(CODE_ILLEGAL_OPERATION);
}
return UtilityException(
(namedErrorCode.isEmpty() ? modErrorCode : namedErrorCode),
(message == NULL || strlen(message) == 0) ?
UTIL_EXCEPTION_CREATE_MESSAGE_CHARS(
cause.getField(Exception::FIELD_MESSAGE)) :
message,
fileNameLiteral, functionNameLiteral, lineNumber,
causeInHandling, typeNameLiteral, stackTraceMode, literalFlags);
}
//------------------------------------------------------------------------------
// Sets the corresponding meta data for the input field name.
//------------------------------------------------------------------------------
bool CCSDSAEMEulerAngleSegment::SetMetaData(const std::string &fieldName,
const std::string &value)
{
#ifdef DEBUG_AEM_EULER_SET
MessageInterface::ShowMessage(
"Entering AEMEulerAngleSegment::SetMetaData with field %s and value %s\n",
fieldName.c_str(), value.c_str());
#endif
if (fieldName == "EULER_ROT_SEQ")
{
// Checking here for valid Euler sequence; however, the standard states
// that, while any sequence is allowed, symmetric ones are not
// recommended. We are not checking for that here at this time.
if (!AttitudeConversionUtility::IsValidEulerSequence(value))
{
std::string errmsg = segError;
errmsg += "Invalid value for field EULER_ROT_SEQ.\n";
throw UtilityException(errmsg);
}
eulerRotSeq = value;
GmatStringUtil::ToInteger(eulerRotSeq.substr(0,1), euler1);
GmatStringUtil::ToInteger(eulerRotSeq.substr(1,1), euler2);
GmatStringUtil::ToInteger(eulerRotSeq.substr(2,1), euler3);
return true;
}
// if it isn't handled here, call the parent class
return CCSDSAEMSegment::SetMetaData(fieldName, value);
}
// -----------------------------------------------------------------------------
// CCSDSEMSegment* GetSegment(Integer num)
// Returns the segment number requested
// -----------------------------------------------------------------------------
CCSDSEMSegment* CCSDSEMReader::GetSegment(Integer num)
{
if ((num < 0) || (num > numSegments))
{
throw UtilityException(
"EphemerisMessage:: segment number requested is out-of-range.");
}
return segments.at(num);
}
//------------------------------------------------------------------------------
// virtual Real Cofactor(int r, int c) const
//------------------------------------------------------------------------------
Real Rmatrix::Cofactor(int r, int c) const
{
#ifdef DEBUG_DETERMINANT
MessageInterface::ShowMessage(wxT("Entering Cofactor with r = %d and c = %d\n"), r, c);
MessageInterface::ShowMessage(wxT(" and rowsD = %d and colsD = %d\n"), rowsD, colsD);
#endif
if (isSizedD == false)
{
throw TableTemplateExceptions::UnsizedTable();
}
if (rowsD != colsD)
throw Rmatrix::NotSquare();
if (rowsD > 9)
{
wxString errmsg = wxT("GMAT Cofactor method not yet optimized. ");
errmsg += wxT("Currently limited to matrices of size 9x9 or smaller.");
throw UtilityException(errmsg);
}
Rmatrix Minor(rowsD - 1, colsD - 1);
Real Cof;
// build the minor matrix
int i, j, minorI, minorJ;
for (i = 0, minorI = -1; i < rowsD; i++)
{
if (i != r)
{
minorI++;
for ( j = 0, minorJ = -1; j < colsD; j++)
{
if (j != c)
{
minorJ++;
Minor(minorI, minorJ) = elementD[i*colsD + j];
} // if (j != c)
} // for (j = ...
} // if (i != r)
} // for (i = ...
#ifdef DEBUG_DETERMINANT
MessageInterface::ShowMessage(wxT("about to call Determinant on minor: \n%s\n"), (Minor.ToString()).c_str());
#endif
Cof = Minor.Determinant();
// if r+c is odd Cof is negative
if ((r+c)%2 == 1)
Cof = - Cof;
return Cof;
}
//------------------------------------------------------------------------------
// Validates the contents of the handled Euler Angle meta data elements.
//------------------------------------------------------------------------------
bool CCSDSAEMEulerAngleSegment::Validate(bool checkData)
{
#ifdef DEBUG_AEM_EULER_VALIDATE
MessageInterface::ShowMessage("Entering CCSDSAEMEulerAngleSegment::Validate\n");
#endif
// Rotation Sequence
if (eulerRotSeq == UNSET_STRING)
{
std::string errmsg = segError;
errmsg += "Required field EULER_ROT_SEQ is missing.\n";
throw UtilityException(errmsg);
}
if (interpolationMethod != "LAGRANGE")
{
std::string errmsg = segError;
errmsg += "Interpolation type \"";
errmsg += interpolationMethod + "\" is not valid for Attitude type ";
errmsg += "EULER_ANGLE. The only supported value is \"LAGRANGE\".\n";
throw UtilityException(errmsg);
}
return CCSDSAEMSegment::Validate(checkData);
}
//------------------------------------------------------------------------------
// Determines the indices of the first and last usable line of data, based
// on the usableStartTime and usableStopTime
//------------------------------------------------------------------------------
bool CCSDSEMSegment::GetUsableIndexRange(Integer &first, Integer &last)
{
#ifdef DEBUG_USABLE
MessageInterface::ShowMessage("In GetUsableIndexRange, usableStart = %12.10f, usableStop = %12.10f\n",
usableStartTime, usableStopTime);
#endif
// If we are not using usableStartTime and usableStopTime, the range
// covers the entire startTime-stopTime span
first = 0;
last = (Integer) dataStore.size() - 1;
// If we are using usableStartTime and usableStopTime, we need to figure
// out which lines of data are included in that span
if (usesUsableTimes)
{
bool firstFound = false;
for (Integer ii = 0; ii < (Integer) dataStore.size(); ii++)
{
Real iiEpoch = dataStore.at(ii)->epoch;
#ifdef DEBUG_USABLE
MessageInterface::ShowMessage(" ii = %d, epoch = %12.10f\n",
ii, iiEpoch);
#endif
if (!firstFound && (iiEpoch > (usableStartTime - EPOCH_MATCH_TOLERANCE)))
{
first = ii;
firstFound = true;
}
else if (iiEpoch > (usableStopTime + EPOCH_MATCH_TOLERANCE))
{
last = ii - 1;
break;
}
}
#ifdef DEBUG_USABLE
MessageInterface::ShowMessage("In GetUsableIndexRange, first = %d, last = %d\n",
first, last);
#endif
}
if (first == last)
{
std::string errmsg = segError;
errmsg += "Only one data point available ";
errmsg += "in usable epoch range.\n";
throw UtilityException(errmsg);
}
return true;
}
//------------------------------------------------------------------------------
void SpiceOrbitKernelReader::GetCoverageStartAndEnd(StringArray &kernels,
Integer forNaifId,
Real &start,
Real &end)
{
// first check to see if a kernel specified is not loaded; if not,
// try to load it
for (unsigned int ii = 0; ii < kernels.size(); ii++)
if (!IsLoaded(kernels.at(ii))) LoadKernel(kernels.at(ii));
SpiceInt idSpice = forNaifId;
SpiceInt arclen = 4;
SpiceInt typlen = 5;
bool firstInt = true;
bool idOnKernel = false;
ConstSpiceChar *kernelName = NULL;
SpiceInt objId = 0;
SpiceInt numInt = 0;
SpiceChar *kernelType;
SpiceChar *arch;
SpiceDouble b;
SpiceDouble e;
Real bA1;
Real eA1;
SPICEINT_CELL(ids, 200);
SPICEDOUBLE_CELL(cover, 200000);
char kStr[5] = " ";
char aStr[4] = " ";
// look through each kernel
for (unsigned int ii = 0; ii < kernels.size(); ii++)
{
#ifdef DEBUG_SPK_COVERAGE
MessageInterface::ShowMessage(wxT("Checking coverage for ID %d on kernel %s\n"),
forNaifId, (kernels.at(ii)).c_str());
#endif
kernelName = kernels[ii].char_str();
// check the type of kernel
arch = aStr;
kernelType = kStr;
getfat_c(kernelName, arclen, typlen, arch, kernelType);
if (failed_c())
{
ConstSpiceChar option[] = "LONG";
SpiceInt numChar = MAX_LONG_MESSAGE_VALUE;
//SpiceChar err[MAX_LONG_MESSAGE_VALUE];
SpiceChar *err = new SpiceChar[MAX_LONG_MESSAGE_VALUE];
getmsg_c(option, numChar, err);
wxString errStr(wxString::FromAscii(err));
wxString errmsg = wxT("Error determining type of kernel \"");
errmsg += kernels.at(ii) + wxT("\". Message received from CSPICE is: ");
errmsg += errStr + wxT("\n");
reset_c();
delete [] err;
throw UtilityException(errmsg);
}
#ifdef DEBUG_SPK_COVERAGE
MessageInterface::ShowMessage(wxT("Kernel is of type %s\n"),
kernelType);
#endif
// only deal with SPK kernels
if (eqstr_c( kernelType, "spk" ))
{
spkobj_c(kernelName, &ids);
// get the list of objects (IDs) for which data exists in the SPK kernel
for (SpiceInt jj = 0; jj < card_c(&ids); jj++)
{
objId = SPICE_CELL_ELEM_I(&ids,jj);
#ifdef DEBUG_SPK_COVERAGE
MessageInterface::ShowMessage(wxT("Kernel contains data for object %d\n"),
(Integer) objId);
#endif
// look to see if this kernel contains data for the object we're interested in
if (objId == idSpice)
{
idOnKernel = true;
break;
}
}
// only deal with kernels containing data for the object we're interested in
if (idOnKernel)
{
#ifdef DEBUG_SPK_COVERAGE
MessageInterface::ShowMessage(wxT("Checking kernel %s for data for object %d\n"),
(kernels.at(ii)).c_str(), (Integer) objId);
#endif
scard_c(0, &cover); // reset the coverage cell
spkcov_c (kernelName, idSpice, &cover);
if (failed_c())
{
ConstSpiceChar option[] = "LONG";
SpiceInt numChar = MAX_LONG_MESSAGE_VALUE;
//SpiceChar err[MAX_LONG_MESSAGE_VALUE];
SpiceChar *err = new SpiceChar[MAX_LONG_MESSAGE_VALUE];
getmsg_c(option, numChar, err);
wxString errStr(wxString::FromAscii(err));
wxString errmsg = wxT("Error determining coverage for SPK kernel \"");
errmsg += kernels.at(ii) + wxT("\". Message received from CSPICE is: ");
errmsg += errStr + wxT("\n");
reset_c();
delete [] err;
throw UtilityException(errmsg);
}
numInt = wncard_c(&cover);
#ifdef DEBUG_SPK_COVERAGE
MessageInterface::ShowMessage(wxT("Number of intervals found = %d\n"),
(Integer) numInt);
#endif
if ((firstInt) && (numInt > 0))
{
wnfetd_c(&cover, 0, &b, &e);
if (failed_c())
{
ConstSpiceChar option[] = "LONG";
SpiceInt numChar = MAX_LONG_MESSAGE_VALUE;
//SpiceChar err[MAX_LONG_MESSAGE_VALUE];
SpiceChar *err = new SpiceChar[MAX_LONG_MESSAGE_VALUE];
getmsg_c(option, numChar, err);
wxString errStr(wxString::FromAscii(err));
wxString errmsg = wxT("Error getting interval times for SPK kernel \"");
errmsg += kernels.at(ii) + wxT("\". Message received from CSPICE is: ");
errmsg += errStr + wxT("\n");
reset_c();
delete [] err;
throw UtilityException(errmsg);
}
start = SpiceTimeToA1(b);
end = SpiceTimeToA1(e);
firstInt = false;
}
for (SpiceInt jj = 0; jj < numInt; jj++)
{
wnfetd_c(&cover, jj, &b, &e);
bA1 = SpiceTimeToA1(b);
eA1 = SpiceTimeToA1(e);
if (bA1 < start) start = bA1;
if (eA1 > end) end = eA1;
}
}
}
}
if (firstInt)
{
wxString errmsg(wxT(""));
errmsg << wxT("Error - no data available for body with NAIF ID ") << forNaifId << wxT(" on specified SPK kernels\n");
throw UtilityException(errmsg);
}
}
//------------------------------------------------------------------------------
// virtual Real Determinant() const
//------------------------------------------------------------------------------
Real Rmatrix::Determinant() const
{
#ifdef DEBUG_DETERMINANT
MessageInterface::ShowMessage(wxT("Entering Determinant with rowsD = %d and colsD = %d\n"), rowsD, colsD);
#endif
if (isSizedD == false)
{
throw TableTemplateExceptions::UnsizedTable();
}
if (rowsD != colsD)
throw Rmatrix::NotSquare();
Real D;
if (rowsD == 1)
{
#ifdef DEBUG_DETERMINANT
MessageInterface::ShowMessage(wxT("Entering Determinant rowsD == 1 clause\n"));
#endif
D = elementD[0];
}
else if (rowsD == 2)
{
#ifdef DEBUG_DETERMINANT
MessageInterface::ShowMessage(wxT("Entering Determinant rowsD == 2 clause\n"));
#endif
D = elementD[0]*elementD[3] - elementD[1]*elementD[2];
}
else if (rowsD == 3)
{
#ifdef DEBUG_DETERMINANT
MessageInterface::ShowMessage(wxT("Entering Determinant rowsD == 3 clause\n"));
#endif
D = elementD[0]*elementD[4]*elementD[8] +
elementD[1]*elementD[5]*elementD[6] +
elementD[2]*elementD[3]*elementD[7] -
elementD[0]*elementD[5]*elementD[7] -
elementD[1]*elementD[3]*elementD[8] -
elementD[2]*elementD[4]*elementD[6];
}
else
{
// Currently limited by inefficiencies in the algorithm
if (rowsD > 9)
{
wxString errmsg = wxT("GMAT Determinant method not yet optimized. ");
errmsg += wxT("Currently limited to matrices of size 9x9 or smaller.");
throw UtilityException(errmsg);
}
#ifdef DEBUG_DETERMINANT
MessageInterface::ShowMessage(wxT("Entering Determinant else clause\n"));
#endif
D = 0.0;
int i;
for (i = 0; i < colsD; i++)
{
Real c = Cofactor(0,i);
#ifdef DEBUG_DETERMINANT
MessageInterface::ShowMessage(wxT("Cofactor(0,%d) = %12.10f\n"), (Integer) i, c);
MessageInterface::ShowMessage(wxT(" now multiplying by element[%d] (%12.10f) to get %12.10f\n"),
(Integer) i, elementD[i], (elementD[i] * c));
#endif
D += elementD[i] * c;
// D += elementD[i]*Cofactor(0,i);
}
#ifdef DEBUG_DETERMINANT
MessageInterface::ShowMessage(wxT("... at end of summation, D = %12.10f\n"), D);
#endif
}
return D;
}
//---------------------------------------------------------------------------
void SpiceAttitudeKernelReader::GetTargetOrientation(const wxString &objectName,
Integer naifID,
Integer forFrameNaifId,
const A1Mjd &atTime,
// Real tolerance,
Rmatrix33 &r33,
Rvector3 &angVel,
const wxString &referenceFrame)
{
#ifdef DEBUG_CK_READING
MessageInterface::ShowMessage(wxT("Entering GetTargetOrientation for object %s, with NAIF ID %d, at time %12.10f, with frame = %s\n"),
objectName.c_str(), naifID, atTime.Get(), referenceFrame.c_str());
#endif
wxString objectNameToUse = objectName;
objectNameToUse = GmatStringUtil::ToUpper(objectNameToUse);
objectNameSPICE = objectNameToUse.char_str();
naifIDSPICE = naifID;
frameNaifIDSPICE = forFrameNaifId;
referenceFrameSPICE = referenceFrame.char_str();
etSPICE = A1ToSpiceTime(atTime.Get());
// boddef_c(objectNameSPICE, naifIDSPICE); // CSPICE method to set NAIF ID for an object - is this valid for spacecraft?
// Convert the time (in TDB) to spacecaft ticks
SpiceDouble scTime;
sce2c_c(naifIDSPICE, etSPICE, &scTime);
if (failed_c())
{
ConstSpiceChar option[] = "LONG"; // retrieve long error message, for now
SpiceInt numChar = MAX_LONG_MESSAGE_VALUE;
//SpiceChar err[MAX_LONG_MESSAGE_VALUE];
SpiceChar *err = new SpiceChar[MAX_LONG_MESSAGE_VALUE];
getmsg_c(option, numChar, err);
wxString errStr(wxString::FromAscii(err));
wxString errmsg = wxT("Error getting spacecraft time (ticks) for object \"");
errmsg += objectName + wxT("\". Message received from CSPICE is: ");
errmsg += errStr + wxT("\n");
reset_c();
delete [] err;
throw UtilityException(errmsg);
}
// get the tolerance in spacecraft clock ticks
wxString tolerance = wxT("01"); // this should probably be user input, or set as a constant
ConstSpiceChar *tol = tolerance.char_str();
SpiceDouble tolTicks;
sctiks_c(naifIDSPICE, tol, &tolTicks);
if (failed_c())
{
ConstSpiceChar option[] = "LONG"; // retrieve long error message, for now
SpiceInt numChar = MAX_LONG_MESSAGE_VALUE;
//SpiceChar err[MAX_LONG_MESSAGE_VALUE];
SpiceChar *err = new SpiceChar[MAX_LONG_MESSAGE_VALUE];
getmsg_c(option, numChar, err);
wxString errStr(wxString::FromAscii(err));
wxString errmsg = wxT("Error getting tolerance (ticks) for object \"");
errmsg += objectName + wxT("\". Message received from CSPICE is: ");
errmsg += errStr + wxT("\n");
reset_c();
delete [] err;
throw UtilityException(errmsg);
}
#ifdef DEBUG_CK_READING
MessageInterface::ShowMessage(wxT("First, check for coverage for object \"%s\", with NAIF ID %d\n"),
objectName.c_str(), naifID);
#endif
Real beginCov = 0.0;
Real endCov = 0.0;
GetCoverageStartAndEnd(loadedKernels, forFrameNaifId, beginCov, endCov, false);
// Now get the C-matrix and angular velocity at the requested time
SpiceDouble cmat[3][3];
SpiceDouble av[3];
SpiceBoolean found;
SpiceDouble clkout;
#ifdef DEBUG_CK_READING
MessageInterface::ShowMessage(wxT("about to call ckgpav: \n"));
MessageInterface::ShowMessage(wxT(" NAIF ID = %d\n")
wxT(" etSPICE = %12.10f\n")
wxT(" scTime = %12.10fn")
wxT(" tolTicks = %12.10f\n")
wxT(" refFrame = %s\n"),
(Integer) naifIDSPICE, (Real) etSPICE, (Real) scTime, (Real) tolTicks,
referenceFrame.c_str());
#endif
ckgpav_c(frameNaifIDSPICE, scTime, tolTicks, referenceFrameSPICE, cmat, av, &clkout, &found);
// ckgpav_c(naifIDSPICE, scTime, tolTicks, referenceFrameSPICE, cmat, av, &clkout, &found);
if (failed_c())
{
ConstSpiceChar option[] = "LONG"; // retrieve long error message, for now
SpiceInt numChar = MAX_LONG_MESSAGE_VALUE;
//SpiceChar err[MAX_LONG_MESSAGE_VALUE];
SpiceChar *err = new SpiceChar[MAX_LONG_MESSAGE_VALUE];
getmsg_c(option, numChar, err);
wxString errStr(wxString::FromAscii(err));
wxString errmsg = wxT("Error getting C-matrix and/or angular velocity for object \"");
errmsg += objectName + wxT("\". Message received from CSPICE is: ");
errmsg += errStr + wxT("\n");
reset_c();
delete [] err;
throw UtilityException(errmsg);
}
if (found == SPICEFALSE)
{
wxString errmsg = wxT("Pointing data for object ");
errmsg += objectName + wxT(" not found on loaded CK/SCLK kernels.\n");
throw UtilityException(errmsg);
}
#ifdef DEBUG_CK_READING
MessageInterface::ShowMessage(wxT("results from ckgpav: \n"));
MessageInterface::ShowMessage(wxT(" cosMat = %12.10f %12.10f %12.10f\n")
wxT(" %12.10f %12.10f %12.10f\n")
wxT(" %12.10f %12.10f %12.10f\n"),
(Real)cmat[0][0], (Real)cmat[0][1], (Real)cmat[0][2],
(Real)cmat[1][0], (Real)cmat[1][1], (Real)cmat[1][2],
(Real)cmat[2][0], (Real)cmat[2][1], (Real)cmat[2][2]);
MessageInterface::ShowMessage(wxT(" angvel = %12.10f %12.10f %12.10f\n"),
(Real)av[0], (Real)av[1], (Real)av[2]);
MessageInterface::ShowMessage(wxT(" and clkout = %12.10f\n"), (Real) clkout);
#endif
// Set output values
r33.Set(cmat[0][0], cmat[0][1], cmat[0][2],
cmat[1][0], cmat[1][1], cmat[1][2],
cmat[2][0], cmat[2][1], cmat[2][2]);
angVel.Set(av[0], av[1], av[2]);
}
//------------------------------------------------------------------------------
void SpiceAttitudeKernelReader::GetCoverageStartAndEnd(StringArray &kernels,
Integer forNaifId,
Real &start,
Real &end,
bool needAngVel)
{
// first check to see if a kernel specified is not loaded; if not,
// try to load it
for (unsigned int ii = 0; ii < kernels.size(); ii++)
if (!IsLoaded(kernels.at(ii))) LoadKernel(kernels.at(ii));
SpiceInt idSpice = forNaifId;
SpiceInt arclen = 4;
SpiceInt typlen = 5;
bool firstInt = true;
bool idOnKernel = false;
char kStr[5] = " ";
char aStr[4] = " ";
char levelStr[8] = "SEGMENT";
char timeStr[4] = "TDB";
SpiceBoolean needAv = needAngVel;
ConstSpiceChar *kernelName = NULL;
ConstSpiceChar *level = levelStr;
ConstSpiceChar *timeSys = timeStr;
SpiceDouble tol = 0.0;
SpiceInt objId = 0;
SpiceInt numInt = 0;
SpiceChar *kernelType;
SpiceChar *arch;
SpiceDouble b;
SpiceDouble e;
Real bA1;
Real eA1;
SPICEINT_CELL(ids, 200);
SPICEDOUBLE_CELL(cover, 200000);
// look through each kernel
for (unsigned int ii = 0; ii < kernels.size(); ii++)
{
#ifdef DEBUG_CK_COVERAGE
MessageInterface::ShowMessage(wxT("Checking coverage for ID %d on kernel %s\n"),
forNaifId, (kernels.at(ii)).c_str());
#endif
kernelName = kernels[ii].char_str();
// check the type of kernel
arch = aStr;
kernelType = kStr;
getfat_c(kernelName, arclen, typlen, arch, kernelType);
if (failed_c())
{
ConstSpiceChar option[] = "LONG";
SpiceInt numChar = MAX_LONG_MESSAGE_VALUE;
SpiceChar err[MAX_LONG_MESSAGE_VALUE];
getmsg_c(option, numChar, err);
wxString errStr(wxString::FromAscii( err));
wxString errmsg = wxT("Error determining type of kernel \"");
errmsg += kernels.at(ii) + wxT("\". Message received from CSPICE is: ");
errmsg += errStr + wxT("\n");
reset_c();
throw UtilityException(errmsg);
}
#ifdef DEBUG_CK_COVERAGE
MessageInterface::ShowMessage(wxT("Kernel is of type %s\n"),
kernelType);
#endif
// only deal with CK kernels
if (eqstr_c(kernelType, "ck") || eqstr_c(kernelType, "CK"))
{
ckobj_c(kernelName, &ids);
// get the list of objects (IDs) for which data exists in the CK kernel
for (SpiceInt jj = 0; jj < card_c(&ids); jj++)
{
objId = SPICE_CELL_ELEM_I(&ids,jj);
#ifdef DEBUG_CK_COVERAGE
MessageInterface::ShowMessage(wxT("Kernel contains data for object %d\n"),
(Integer) objId);
#endif
// look to see if this kernel contains data for the object we're interested in
if (objId == idSpice)
{
idOnKernel = true;
break;
}
// if (objId == (idSpice * 1000))
// {
// idSpice = idSpice * 1000;
// naifIDSPICE = idSpice; // not the way to do this - should pass it back
// idOnKernel = true;
// break;
// }
}
// only deal with kernels containing data for the object we're interested in
if (idOnKernel)
{
#ifdef DEBUG_CK_COVERAGE
MessageInterface::ShowMessage(wxT("Checking kernel %s for data for object %d\n"),
(kernels.at(ii)).c_str(), (Integer) objId);
#endif
scard_c(0, &cover); // reset the coverage cell
ckcov_c (kernelName, idSpice, needAv, level, tol, timeSys, &cover);
if (failed_c())
{
ConstSpiceChar option[] = "LONG";
SpiceInt numChar = MAX_LONG_MESSAGE_VALUE;
SpiceChar err[MAX_LONG_MESSAGE_VALUE];
getmsg_c(option, numChar, err);
wxString errStr(wxString::FromAscii(err));
wxString errmsg = wxT("Error determining coverage for CK kernel \"");
errmsg += kernels.at(ii) + wxT("\". Message received from CSPICE is: ");
errmsg += errStr + wxT("\n");
reset_c();
throw UtilityException(errmsg);
}
numInt = wncard_c(&cover);
#ifdef DEBUG_CK_COVERAGE
MessageInterface::ShowMessage(wxT("Number of intervals found = %d\n"),
(Integer) numInt);
#endif
if ((firstInt) && (numInt > 0))
{
wnfetd_c(&cover, 0, &b, &e);
if (failed_c())
{
ConstSpiceChar option[] = "LONG";
SpiceInt numChar = MAX_LONG_MESSAGE_VALUE;
SpiceChar err[MAX_LONG_MESSAGE_VALUE];
getmsg_c(option, numChar, err);
wxString errStr(wxString::FromAscii(err));
wxString errmsg = wxT("Error getting interval times for CK kernel \"");
errmsg += kernels.at(ii) + wxT("\". Message received from CSPICE is: ");
errmsg += errStr + wxT("\n");
reset_c();
throw UtilityException(errmsg);
}
start = SpiceTimeToA1(b);
end = SpiceTimeToA1(e);
firstInt = false;
}
for (SpiceInt jj = 0; jj < numInt; jj++)
{
wnfetd_c(&cover, jj, &b, &e);
bA1 = SpiceTimeToA1(b);
eA1 = SpiceTimeToA1(e);
if (bA1 < start) start = bA1;
if (eA1 > end) end = eA1;
}
}
}
}
if (firstInt)
{
char itsName[256];
SpiceChar *itsNameSPICE = itsName;
SpiceBoolean found2;
bodc2n_c(naifIDSPICE, 256, itsNameSPICE, &found2);
if (found2 == SPICEFALSE)
{
wxString errmsg = wxT("Error - unable to find name for body in SPICE kernel pool");
throw UtilityException(errmsg);
}
else
{
wxString nameStr = wxString::FromAscii(itsNameSPICE);
wxString errmsg = wxT("Error - no data available for body ");
errmsg += nameStr + wxT(" on specified CK kernels");
throw UtilityException(errmsg);
}
}
}
// -----------------------------------------------------------------------------
// bool ParseFile()
// Parse the file, validating where possible
// -----------------------------------------------------------------------------
bool CCSDSEMReader::ParseFile()
{
if (isInitialized) return true;
// Open the file for reading
std::string line;
std::ifstream ephFile(emFile.c_str());
if (!ephFile) return false;
// Ignore leading white space
ephFile.setf(std::ios::skipws);
// check for an empty file
if (ephFile.eof())
{
std::string errmsg = "Error reading ephemeris message file \"";
errmsg += emFile + "\". ";
errmsg += "File appears to be empty.\n";
throw UtilityException(errmsg);
}
// Read the header data first - version number must be
// on the first non-blank line
std::string firstWord, firstAllCaps;
std::string eqSign;
// read the first line for the version number
getline(ephFile,line);
#ifdef DEBUG_PARSE_EM_FILE
MessageInterface::ShowMessage("In CCSDSEMReader, line= %s\n", line.c_str());
#endif
std::istringstream lineStr;
lineStr.str(line);
lineStr >> firstWord;
firstAllCaps = GmatStringUtil::ToUpper(firstWord);
if (firstAllCaps == versionFieldName)
{
lineStr >> eqSign;
if (eqSign != "=")
{
std::string errmsg = "Error reading ephemeris message file \"";
errmsg += emFile + "\". ";
errmsg += "Equal sign missing or incorrect.\n";
throw UtilityException(errmsg);
}
std::string versionValue;
lineStr >> versionValue;
if (!GmatStringUtil::IsNumber(versionValue))
{
std::string errmsg = "Error reading ephemeris message file \"";
errmsg += emFile + "\". ";
errmsg += "Version number is not a valid real number.\n";
throw UtilityException(errmsg);
}
if (!IsValidVersion(versionValue))
{
std::string errmsg = "Error reading ephemeris message file \"";
errmsg += emFile + "\". ";
errmsg += "Version number is not valid.\n";
throw UtilityException(errmsg);
}
versionNumber = versionValue;
versionFound = true;
}
//------------------------------------------------------------------------------
// Determines whether or not the input time exactly matches an epoch from the
// dataStore. If so, it returns the state at that epoch. If not, and the
// interpolation degree = 0, it returns the last data before the input epoch.
// Otherwise, it calls the interpolation method to get the state.
//------------------------------------------------------------------------------
Rvector CCSDSEMSegment::DetermineState(Real atEpoch)
{
#ifdef DEBUG_DETERMINE_STATE
MessageInterface::ShowMessage("In DetermineState, epoch = %12.10f\n", atEpoch);
#endif
// Make sure that if we are using usable times, we only check times
// between usableStartTime and usableStopTime
if (usesUsableTimes &&
((atEpoch < (usableStartTime - EPOCH_MATCH_TOLERANCE)) ||
(atEpoch > (usableStopTime + EPOCH_MATCH_TOLERANCE))))
{
std::ostringstream errmsg;
errmsg.precision(16);
errmsg << "Specified epoch ";
errmsg << atEpoch << " in segment ";
errmsg << segmentNumber << " must be within usable time range.\n";
throw UtilityException(errmsg.str());
}
bool exactMatchFound = false;
Integer matchPos = -1;
for (unsigned int ii= 0; ii < dataStore.size(); ii++)
{
Real theTime = (dataStore.at(ii))->epoch;
#ifdef DEBUG_EM_FIND_EXACT_MATCH
MessageInterface::ShowMessage("---- data epoch(%d) = %12.10f \n",
ii, theTime);
#endif
if (GmatMathUtil::IsEqual(theTime, atEpoch, EPOCH_MATCH_TOLERANCE))
{
exactMatchFound = true;
matchPos = ii;
#ifdef DEBUG_EM_FIND_EXACT_MATCH
MessageInterface::ShowMessage("---- EXACT MATCH to epoch = %12.10f\n",
(dataStore.at(ii))->epoch);
#endif
break;
}
if (theTime < atEpoch) matchPos = ii;
// if the time is bigger, we've gone past and won't find an exact match
else if (theTime > atEpoch) break;
}
// if we didn't find an exact match OR an epoch less than the input
// epoch, that is an error
if (matchPos < 0)
{
std::ostringstream errmsg;
errmsg.precision(16);
errmsg << "Error searching for epoch ";
errmsg << atEpoch << " in segment ";
errmsg << segmentNumber << ", within usable time range.\n";
throw UtilityException(errmsg.str());
}
if (exactMatchFound || (interpolationDegree == 0))
return (dataStore.at(matchPos))->data;
else
{
#ifdef DEBUG_DETERMINE_STATE
MessageInterface::ShowMessage("In DetermineState, about to interpolate to time %12.10f\n", atEpoch);
#endif
return Interpolate(atEpoch);
}
}
//------------------------------------------------------------------------------
// Sets the corresponding meta data for the input field name.
//------------------------------------------------------------------------------
bool CCSDSEMSegment::SetMetaData(const std::string &fieldName,
const std::string &value)
{
#ifdef DEBUG_EM_SET_META
MessageInterface::ShowMessage(
"Entering EMSegment::SetMetaData with field %s and value %s\n",
fieldName.c_str(), value.c_str());
#endif
Real theTime;
if (fieldName == "COMMENT")
{
metaComments.push_back(value);
return true;
}
else if (fieldName == "OBJECT_NAME")
{
objectName = value;
return true;
}
else if (fieldName == "OBJECT_ID")
{
objectID = value;
return true;
}
else if (fieldName == "CENTER_NAME")
{
centerName = value;
return true;
}
// @todo - check for this:
// "The TIME_SYSTEM value must remain fixed within an AEM."
else if (fieldName == "TIME_SYSTEM")
{
timeSystem = GmatStringUtil::ToUpper(value);
return true;
}
else if (fieldName == "START_TIME")
{
theTime = ParseEpoch(value);
startTime = theTime;
return true;
}
else if (fieldName == "USEABLE_START_TIME")
{
theTime = ParseEpoch(value);
usableStartTime = theTime;
return true;
}
else if (fieldName == "USEABLE_STOP_TIME")
{
theTime = ParseEpoch(value);
usableStopTime = theTime;
return true;
}
else if (fieldName == "STOP_TIME")
{
theTime = ParseEpoch(value);
stopTime = theTime;
return true;
}
else if (fieldName == "INTERPOLATION_DEGREE")
{
Integer theDegree;
std::string trimmedValue = GmatStringUtil::Trim(value, GmatStringUtil::BOTH, true, true);
bool isInt = GmatStringUtil::ToInteger(trimmedValue, theDegree);
if (!isInt)
{
std::string errmsg = segError;
errmsg += "Value for INTERPOLATION_DEGREE is not ";
errmsg += "a valid integer\n";
throw UtilityException(errmsg);
}
interpolationDegree = theDegree;
return true;
}
return false;
}
//------------------------------------------------------------------------------
// Parse a time string read from the EM file and convert it to
// a Real (A1Mjd) epoch
//------------------------------------------------------------------------------
Real CCSDSEMSegment::ParseEpoch(const std::string &epochString)
{
#ifdef DEBUG_PARSE_EPOCH
MessageInterface::ShowMessage("ParseEpoch -- epochString = \"%s\"\n",
epochString.c_str());
#endif
// The epochs can be in either of two formats:
// YYYY-MM-DDThh:mm:ss.mmm
// YYYY-DOYThh:mm:ss
std::string yyyymmdd, hhmmss, YYstr, MMstr, DDstr, hhstr, mmstr, ssmmmstr;
Integer year, month = 0, day, hour, minute;
Real seconds;
bool doyUsed = false;
std::size_t tPos = epochString.find_first_of("Tt");
if (tPos == std::string::npos)
{
std::string errmsg = "Error reading ephemeris message file segment. ";
errmsg += "Missing or incorrectly formatted data Epoch.\n";
throw UtilityException(errmsg);
}
yyyymmdd = epochString.substr(0,tPos);
hhmmss = epochString.substr(tPos+1);
if ((hhmmss.length() < 8) ||
(GmatStringUtil::NumberOfOccurrences(hhmmss, ':') != 2))
{
std::string errmsg = "Error reading ephemeris message file segment. ";
errmsg += "hhmmss.sss part of Time \"" + epochString;
errmsg += "\" is not formatted correctly.\n";
throw UtilityException(errmsg);
}
hhstr = hhmmss.substr(0,2);
mmstr = hhmmss.substr(3,2);
ssmmmstr = hhmmss.substr(6);
if ((!GmatStringUtil::ToInteger(hhstr, hour)) ||
(!GmatStringUtil::ToInteger(mmstr, minute)) ||
(!GmatStringUtil::ToReal(ssmmmstr, seconds)))
{
std::string errmsg = "Error reading ephemeris message file segment. ";
errmsg += "hhmmss.sss part of Time \"" + epochString;
errmsg += "\" is not formatted correctly.\n";
throw UtilityException(errmsg);
}
Integer numDashes = GmatStringUtil::NumberOfOccurrences(yyyymmdd, '-');
if (numDashes == 1) // YYYY-DOYThh:mm:ss
{
doyUsed = true;
YYstr = yyyymmdd.substr(0,4);
DDstr = yyyymmdd.substr(5,3);
if ((!GmatStringUtil::ToInteger(YYstr, year)) ||
(!GmatStringUtil::ToInteger(DDstr, day)))
{
std::string errmsg = "Error reading ephemeris message file segment. ";
errmsg += "YYYY-DOY part of Time \"" + epochString;
errmsg += "\" is not formatted correctly.\n";
throw UtilityException(errmsg);
}
}
else if (numDashes == 2) // YYYY-MM-DDThh:mm:ss.mmm
{
doyUsed = false;
YYstr = yyyymmdd.substr(0,4);
MMstr = yyyymmdd.substr(5,2);
DDstr = yyyymmdd.substr(8,2);
if ((!GmatStringUtil::ToInteger(YYstr, year)) ||
(!GmatStringUtil::ToInteger(MMstr, month)) ||
(!GmatStringUtil::ToInteger(DDstr, day)))
{
std::string errmsg = "Error reading ephemeris message file segment. ";
errmsg += "YYYY-MM-DD part of Time \"" + epochString;
errmsg += "\" is not formatted correctly.\n";
throw UtilityException(errmsg);
}
}
else
{
std::string errmsg = "Error reading ephemeris message file segment. ";
errmsg += "Time \"" + epochString;
errmsg += "\" is not formatted correctly.\n";
throw UtilityException(errmsg);
}
#ifdef DEBUG_PARSE_EPOCH
MessageInterface::ShowMessage("epochString = %s, yyyymmdd = %s, hhmmss = %s\n",
epochString.c_str(), yyyymmdd.c_str(), hhmmss.c_str());
MessageInterface::ShowMessage("year = %d, month = %d, day = %d\n",
year, month, day);
MessageInterface::ShowMessage("hour = %d, minute = %d, seconds = %lf\n",
hour, minute, seconds);
#endif
if (doyUsed)
{
try
{
UtcDate utc(year, day, hour, minute, seconds);
#ifdef DEBUG_PARSE_EPOCH
MessageInterface::ShowMessage(" --- UTC data to packed calendar string = %s\n",
utc.ToPackedCalendarString().c_str());
#endif
return utc.ToA1Mjd();
}
catch (Date::LeapYearError &lye)
{
std::ostringstream errmsg;
errmsg << "Cannot read CCSDS file. File contains time \"";
errmsg << epochString << "\", which specifies day number ";
errmsg << day << " for a non-leap year.\n";
throw UtilityException(errmsg.str());
}
}
else
{
UtcDate utc(year, month, day, hour, minute, seconds);
#ifdef DEBUG_PARSE_EPOCH
MessageInterface::ShowMessage(" --- UTC data to packed calendar string = %s\n",
utc.ToPackedCalendarString().c_str());
#endif
return utc.ToA1Mjd();
}
}
//------------------------------------------------------------------------------
// Rvector InterpolateSLERP(Real atEpoch)
// Interpolates the segment data using SLERP interpolation.
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
Rvector CCSDSEMSegment::InterpolateSLERP(Real atEpoch)
{
// Adapted from MATLAB version:
// YRL, Interpolate Quaternion using SLERP
#ifdef DEBUG_SLERP
MessageInterface::ShowMessage(">>>> Entering SLERP (%d), atEpoch = %12.10f\n",
segmentNumber, atEpoch);
#endif
// Sanity Checks
#ifdef DEBUG_SLERP
MessageInterface::ShowMessage(">>>> dataStore size = %d, first = %d, last = %d\n",
(Integer) dataStore.size(), firstUsable, lastUsable);
MessageInterface::ShowMessage(">>>> first time = %12.10f, last time = %12.10f\n",
dataStore.at(firstUsable)->epoch, dataStore.at(lastUsable)->epoch);
MessageInterface::ShowMessage(">>>> and requested epoch = %12.10f\n",
atEpoch);
#endif
Real minEpoch = dataStore.at(firstUsable)->epoch;
Real maxEpoch = dataStore.at(lastUsable)->epoch;
if ((atEpoch < minEpoch) || (atEpoch > maxEpoch))
{
std::string errmsg = "Requested time for SLERP interpolation ";
errmsg += "is out of usable epoch range.\n";
throw UtilityException(errmsg);
}
// Interpolation Algorithm (SLERP)
// find correct (first largest) epoch in ephemeris data
Real anEpoch = 0.0;
Integer epochPos = 0;
for (Integer ii = firstUsable; ii <= lastUsable; ii++)
{
anEpoch = (dataStore.at(ii))->epoch;
if ( anEpoch > atEpoch)
{
epochPos = ii;
break;
}
}
#ifdef DEBUG_SLERP
MessageInterface::ShowMessage("In SLERP, minEpoch = %12.10f\n", minEpoch);
MessageInterface::ShowMessage("In SLERP, maxEpoch = %12.10f\n", maxEpoch);
MessageInterface::ShowMessage("In SLERP, epochPos = %d\n", epochPos);
#endif
// Get times and data for points before and after requested time
Real t1 = (dataStore.at(epochPos-1))->epoch;
Real t2 = anEpoch;
Rvector d1 = (dataStore.at(epochPos-1))->data;
Rvector d2 = (dataStore.at(epochPos))->data;
#ifdef DEBUG_SLERP
MessageInterface::ShowMessage("In SLERP, t1 = %12.10f\n", t1);
MessageInterface::ShowMessage("In SLERP, t2 = %12.10f\n", t2);
MessageInterface::ShowMessage("In SLERP, d1 = \n");
for (Integer ii = 0; ii < (Integer) d1.GetSize(); ii++)
MessageInterface::ShowMessage(" %12.10f\n", d1[ii]);
MessageInterface::ShowMessage("In SLERP, d2 = \n");
for (Integer ii = 0; ii < (Integer) d2.GetSize(); ii++)
MessageInterface::ShowMessage(" %12.10f\n", d2[ii]);
#endif
Real cosOmega = ( d1 * d2 ) / d1.GetMagnitude() / d2.GetMagnitude() ;
Real sinOmega = GmatMathUtil::Sqrt(1.0 - GmatMathUtil::Pow(cosOmega,2));
Real omega = GmatMathUtil::ASin(sinOmega) ;
Real t = ( atEpoch - t1 ) / ( t2 - t1 ) ;
Rvector dSlerp(dataSize);
#ifdef DEBUG_SLERP
MessageInterface::ShowMessage("In SLERP, t = %12.10f\n", t);
MessageInterface::ShowMessage("In SLERP, cosOmega = %12.10f\n", cosOmega);
MessageInterface::ShowMessage("In SLERP, sinOmega = %12.10f\n", sinOmega);
MessageInterface::ShowMessage("In SLERP, omega = %12.10f\n", omega);
#endif
for (Integer jj = 0; jj < dataSize; jj++)
{
if (sinOmega == 0)
dSlerp[jj] = (1-t)*d1[jj] + t*d2[jj] ;
else
dSlerp[jj] = (GmatMathUtil::Sin((1-t)*omega) * d1[jj] + GmatMathUtil::Sin(t*omega) * d2[jj]) / sinOmega;
}
return dSlerp;
}
//------------------------------------------------------------------------------
// Rvector InterpolateLagrange(Real atEpoch)
// Interpolates the segment data using Lagrange interpolation.
//------------------------------------------------------------------------------
Rvector CCSDSEMSegment::InterpolateLagrange(Real atEpoch)
{
#ifdef DEBUG_INTERP_DATA
MessageInterface::ShowMessage("\n atEpoch = %12.10f, ", atEpoch);
MessageInterface::ShowMessage(" dataStore size = %d", (Integer) dataStore.size());
MessageInterface::ShowMessage(" firstUsable index = %d, last Usable index = %d\n",
firstUsable, lastUsable);
#endif
// Adapted from MATLAB version:
// Author: Joel J. K. Parker <*****@*****.**>
// Input parsing
Integer n = interpolationDegree;
// Sanity Checks
Real minEpoch = dataStore.at(firstUsable)->epoch;
Real maxEpoch = dataStore.at(lastUsable)->epoch;
#ifdef DEBUG_INTERP_DATA
MessageInterface::ShowMessage("\n n = %d, ", n);
MessageInterface::ShowMessage(" min epoch = %12.10f", minEpoch);
MessageInterface::ShowMessage(" max epoch = %12.10f", maxEpoch);
#endif
if ((atEpoch < (minEpoch - EPOCH_MATCH_TOLERANCE)) ||
(atEpoch > (maxEpoch + EPOCH_MATCH_TOLERANCE)))
{
std::string errmsg = "Requested time for LAGRANGE interpolation ";
errmsg += "is out of usable epoch range.\n";
throw UtilityException(errmsg);
}
// Integer numStates = dataStore.size();
// The number of usable states we have
Integer numStates = lastUsable - firstUsable + 1;
#ifdef DEBUG_INTERP_DATA
MessageInterface::ShowMessage("\n numStates = %d, ", numStates);
#endif
if (n >= numStates)
{
std::string errmsg = "Insufficient usable data for LAGRANGE interpolation.\n";
throw UtilityException(errmsg);
}
// Initialization
Rvector state(dataSize); // Creates a vector of zeroes
// find intended position of epoch in ephemeris data
// find correct (first largest) epoch in ephemeris data
Real anEpoch = 0.0;
Integer epochPos = 0;
for (Integer ii = firstUsable; ii <= lastUsable; ii++)
{
anEpoch = (dataStore.at(ii))->epoch;
if ( anEpoch > atEpoch)
{
epochPos = ii;
break;
}
}
Integer initIndex = -1;
// pick starting point for interpolation data
// (region ending just before epoch's position in the ephemeris)
if (n >= (epochPos - firstUsable)) // was n + 1 and just epochPos
initIndex = firstUsable; // + 1; was +1 // was 1
else
initIndex = epochPos - n; // was - (n+1);
#ifdef DEBUG_INTERP_DATA
MessageInterface::ShowMessage("first epoch > input epoch is at index %d\n", epochPos);
MessageInterface::ShowMessage("initIndex = %d\n", initIndex);
MessageInterface::ShowMessage("INTEGER_MAX = %d\n", GmatRealConstants::INTEGER_MAX);
#endif
// slide interpolation data region forward until epoch is nearest
// numerical center
Real pDiff = GmatRealConstants::REAL_MAX;
Integer q = -1;;
Real diff = -999.99;
for (Integer ii = initIndex; ii < (numStates-n); ii++) // was <= (numStates-n); ii++)
{
Real dataEpoch = dataStore.at(ii)->epoch;
Real dataEpochN = dataStore.at(ii+n)->epoch;
diff = GmatMathUtil::Abs((dataEpoch + dataEpochN) / 2 - atEpoch);
#ifdef DEBUG_INTERP_DATA_DETAIL
if (atEpoch > 21545.070626)
{
MessageInterface::ShowMessage("--- ii = %d\n", ii);
MessageInterface::ShowMessage("----- epoch at ii = %12.10f\n", dataEpoch);
MessageInterface::ShowMessage("----- epoch at ii+n = %12.10f\n", dataEpochN);
MessageInterface::ShowMessage("----- pDiff= %12.10f\n", pDiff);
MessageInterface::ShowMessage("----- diff = %12.10f\n", diff);
}
#endif
if (diff > pDiff) break;
else q = ii;
pDiff = diff;
}
Rvector d1(dataSize);
Real t1, t2;
#ifdef DEBUG_INTERP_DATA
MessageInterface::ShowMessage("\n Interpolating over %d (q) to %d (q+n)\n", q, (q+n));
MessageInterface::ShowMessage(" epoch and Data at those points are:\n");
for (Integer jj = q; jj <= q+n; jj++)
{
// this is specific to Euler angles here ...
Rvector theAngles = dataStore.at(jj)->data;
Real theEpoch = dataStore.at(jj)->epoch;
MessageInterface::ShowMessage(" %d %12.10f %12.10f %12.10f %12.10f\n",
jj, theEpoch,
theAngles[0] * GmatMathConstants::DEG_PER_RAD,
theAngles[1] * GmatMathConstants::DEG_PER_RAD,
theAngles[2] * GmatMathConstants::DEG_PER_RAD);
}
#endif
for (Integer ii = q; ii <= q+n; ii++)
{
t1 = dataStore.at(ii)->epoch;
d1 = dataStore.at(ii)->data;
#ifdef DEBUG_INTERP_DATA_DETAIL
if (atEpoch > 21545.070626)
{
MessageInterface::ShowMessage("Using epoch %12.10f and data: ", t1);
for (Integer nn = 0; nn < dataSize; nn++)
MessageInterface::ShowMessage(" %12.10f ", d1[nn] * GmatMathConstants::DEG_PER_RAD);
MessageInterface::ShowMessage("\n");
}
#endif
for (Integer jj = q; jj <= q+n; jj++)
{
t2 = dataStore.at(jj)->epoch;
if (ii != jj)
d1 = d1 * ( (atEpoch - t2) / (t1 - t2) );
}
state = state + d1;
}
return state;
}
//------------------------------------------------------------------------------
Rvector6 SpiceOrbitKernelReader::GetTargetState(const wxString &targetName,
const Integer targetNAIFId,
const A1Mjd &atTime,
const wxString &observingBodyName,
const wxString &referenceFrame,
const wxString &aberration)
{
#ifdef DEBUG_SPK_READING
MessageInterface::ShowMessage(
wxT("Entering SPKReader::GetTargetState with target = %s, naifId = %d, time = %12.10f, observer = %s\n"),
targetName.c_str(), targetNAIFId, atTime.Get(), observingBodyName.c_str());
Real start, end;
GetCoverageStartAndEnd(loadedKernels, targetNAIFId, start, end);
MessageInterface::ShowMessage(wxT(" coverage for object %s : %12.10f --> %12.10f\n"),
targetName.c_str(), start, end);
#endif
wxString targetNameToUse = GmatStringUtil::ToUpper(targetName);
if (targetNameToUse == wxT("LUNA")) // We use Luna, instead of Moon, for GMAT
targetNameToUse = wxT("MOON");
if (targetNameToUse == wxT("SOLARSYSTEMBARYCENTER"))
targetNameToUse = wxT("SSB");
objectNameSPICE = targetNameToUse.char_str();
observingBodyNameSPICE = observingBodyName.char_str();
referenceFrameSPICE = referenceFrame.char_str();
aberrationSPICE = aberration.char_str();
// convert time to Ephemeris Time (TDB)
etSPICE = A1ToSpiceTime(atTime.Get());
naifIDSPICE = targetNAIFId;
boddef_c(objectNameSPICE, naifIDSPICE); // CSPICE method to set NAIF ID for an object
#ifdef DEBUG_SPK_READING
MessageInterface::ShowMessage(wxT("SET NAIF Id for object %s to %d\n"),
targetNameToUse.c_str(), targetNAIFId);
// MessageInterface::ShowMessage(
// wxT("In SPKReader::Converted (to TBD) time = %12.10f\n"), etMjdAtTime);
// MessageInterface::ShowMessage(wxT(" then the full JD = %12.10f\n"),
// (etMjdAtTime + GmatTimeConstants::JD_JAN_5_1941));
MessageInterface::ShowMessage(wxT("So time passed to SPICE is %12.14f\n"), (Real) etSPICE);
#endif
SpiceDouble state[6];
SpiceDouble oneWayLightTime;
spkezr_c(objectNameSPICE, etSPICE, referenceFrameSPICE, aberrationSPICE,
observingBodyNameSPICE, state, &oneWayLightTime);
#ifdef DEBUG_SPK_PLANETS
Real ttMjdAtTime = TimeConverterUtil::Convert(atTime.Get(), TimeConverterUtil::A1MJD,
TimeConverterUtil::TTMJD, GmatTimeConstants::JD_JAN_5_1941);
// Real etJd = etMjdAtTime + GmatTimeConstants::JD_JAN_5_1941;
Real ttJd = ttMjdAtTime + GmatTimeConstants::JD_JAN_5_1941;
MessageInterface::ShowMessage(wxT("Asking CSPICE for state of body %s, with observer %s, referenceFrame %s, and aberration correction %s\n"),
objectNameSPICE, observingBodyNameSPICE, referenceFrameSPICE, aberrationSPICE);
MessageInterface::ShowMessage(
wxT(" Body: %s TT Time: %12.10f TDB Time: %12.10f state: %12.10f %12.10f %12.10f %12.10f %12.10f %12.10f\n"),
targetName.c_str(), ttJd, /*etJd,*/ state[0], state[1], state[2], state[3], state[4], state[5]);
#endif
if (failed_c())
{
// ConstSpiceChar option[] = wxT("SHORT"); // retrieve short error message, for now
// SpiceInt numChar = MAX_SHORT_MESSAGE;
// SpiceChar err[MAX_SHORT_MESSAGE];
ConstSpiceChar option[] = "LONG"; // retrieve long error message, for now
SpiceInt numChar = MAX_LONG_MESSAGE_VALUE;
//SpiceChar err[MAX_LONG_MESSAGE_VALUE];
SpiceChar *err = new SpiceChar[MAX_LONG_MESSAGE_VALUE];
getmsg_c(option, numChar, err);
wxString errStr(wxString::FromAscii(err));
wxString errmsg = wxT("Error getting state for body \"");
errmsg += targetName + wxT("\". Message received from CSPICE is: ");
errmsg += errStr + wxT("\n");
reset_c();
delete [] err;
throw UtilityException(errmsg);
}
#ifdef DEBUG_SPK_READING
MessageInterface::ShowMessage(
wxT("In SPKReader::Called spkezr_c and got state out\n"));
#endif
Rvector6 r6(state[0],state[1],state[2],state[3],state[4],state[5]);
return r6;
}
// -----------------------------------------------------------------------------
// void Initialize()
// -----------------------------------------------------------------------------
void CCSDSEMReader::Initialize()
{
if (isInitialized) return;
#ifdef DEBUG_INIT_EM_FILE
MessageInterface::ShowMessage("Entering CCSDSEMReader::Initialize ...\n");
#endif
// Check for the existence of the file
if (!GmatFileUtil::DoesFileExist(emFile))
{
std::string errmsg = "Specified ephemeris message file \" ";
errmsg += emFile + "\" does not exist.\n";
throw UtilityException(errmsg);
}
// Parse the file
bool fileParsed = ParseFile();
if (!fileParsed)
{
std::string errmsg = "There is an error opening or reading the ";
errmsg += "ephemeris message file \"" + emFile + "\"\n";
throw UtilityException(errmsg);
}
#ifdef DEBUG_INIT_EM_FILE
MessageInterface::ShowMessage(" in CCSDSEMReader::Initialize, file has been parsed.\n");
#endif
// Validate the header data first
if (versionNumber == "0.0")
{
std::string errmsg = "Error reading ephemeris message file \"";
errmsg += emFile + "\". ";
errmsg += "Version number is missing or invalid.\n";
throw UtilityException(errmsg);
}
// These we ignore, so we won't write error messages for this, for now
// if (originator == "")
// {
// std::string errmsg = "Error reading ephemeris message file \"";
// errmsg += emFile + "\". ";
// errmsg += "ORIGINATOR is missing or invalid.\n";
// throw UtilityException(errmsg);
// }
// if (creationDate == "")
// {
// std::string errmsg = "Error reading ephemeris message file \"";
// errmsg += emFile + "\". ";
// errmsg += "CREATION_DATE is missing or invalid.\n";
// throw UtilityException(errmsg);
// }
if (numSegments == 0)
{
std::string errmsg = "Error reading ephemeris message file \"";
errmsg += emFile + "\". ";
errmsg += "No segments found.\n";
throw UtilityException(errmsg);
}
#ifdef DEBUG_INIT_EM_FILE
MessageInterface::ShowMessage(" in CCSDSEMReader::Initialize, about to validate segments.\n");
#endif
// Validate all of the segments
bool segmentOK = true;
for (Integer ii = 0; ii < numSegments; ii++)
{
#ifdef DEBUG_INIT_EM_FILE
MessageInterface::ShowMessage(
" in CCSDSEMReader::Initialize, about to validate segment %d <%p>.\n",
ii, segments.at(ii));
#endif
segmentOK = (segments.at(ii))->Validate();
if (!segmentOK)
{
std::stringstream errmsg("");
errmsg << "There is an error validating segment ";
errmsg << ii << ", contained in file " << emFile << ".\n";
throw UtilityException(errmsg.str());
}
}
#ifdef DEBUG_INIT_EM_FILE
MessageInterface::ShowMessage(" in CCSDSEMReader::Initialize, segments have been validated.\n");
#endif
// Validate that the segments' start and stop times are in the correct order
Real currentStop = (segments.at(0))->GetStopTime();
Real segStart, segStop;
for (Integer jj = 1; jj < numSegments; jj++)
{
segStart = (segments.at(jj))->GetStartTime();
segStop = (segments.at(jj))->GetStopTime();
#ifdef DEBUG_INIT_EM_FILE
MessageInterface::ShowMessage("--- last segment stop time = %12.10f, segment start = %12.10f\n",
currentStop, segStart);
#endif
if (segStart < currentStop)
{
std::string errmsg = "Error reading ephemeris message file \"";
errmsg += emFile + "\". ";
errmsg += "Segment start and stop times are not properly ordered.\n";
throw UtilityException(errmsg);
currentStop = segStop;
}
}
#ifdef DEBUG_INIT_EM_FILE
MessageInterface::ShowMessage(" in CCSDSEMReader::Initialize, start and stop times have been checked.\n");
#endif
isInitialized = true;
}
//------------------------------------------------------------------------------
// bool Initialize()
//------------------------------------------------------------------------------
bool LeapSecsFileReader::Initialize()
{
std::ifstream instream;
try
{
if (!isInitialized)
{
instream.open(withFileName.c_str());
//if (instream == NULL)
if (!instream.is_open())
{
std::string errMsg = "Unable to locate leap second file "
+ withFileName + "\n";
throw UtilityException(errMsg);
}
bool isOK = true;
Integer numLinesParsed = 0;
while (!instream.eof())
{
std::string line;
getline(instream,line);
if (!GmatStringUtil::IsBlank(line, true))
{
if (!(isOK = Parse(line))) break;
numLinesParsed++;
}
}
// Why personalization data written to this file?
// Try closing before throwing an exception (LOJ: 2014.06.18)
// Moved from below
#ifdef DEBUG_LEAP_SECOND_FILE
MessageInterface::ShowMessage
("LeapSecsFileReader::Initialize() 1 Closing leap second file\n");
#endif
instream.close();
if (!isOK)
{
std::string errMsg = "Unable to read leap second file "
+ withFileName + " - file is malformed\n";
throw UtilityException(errMsg);
}
if (numLinesParsed == 0)
{
std::string errMsg = "Unable to read leap second file "
+ withFileName + " - file contains no data\n";
throw UtilityException(errMsg);
}
//instream.close();
}
}
catch (...)
{
if (instream.is_open())
{
#ifdef DEBUG_LEAP_SECOND_FILE
MessageInterface::ShowMessage
("LeapSecsFileReader::Initialize() 2 Closing leap second file\n");
#endif
instream.close();
}
//MessageInterface::PopupMessage(Gmat::WARNING_,
// "Unknown Error in LeapSecondFileReader");
// re-throw the exception
throw;
}
isInitialized = true;
return isInitialized;
}
//------------------------------------------------------------------------------
// Validates the contents of the handled common meta data elements and checks
// for the data.
//------------------------------------------------------------------------------
bool CCSDSEMSegment::Validate(bool checkData)
{
#ifdef DEBUG_EM_VALIDATE
MessageInterface::ShowMessage("Entering EMSegment::Validate ...\n");
#endif
// Time system
if (timeSystem == UNSET_STRING)
{
std::string errmsg = segError;
errmsg += "Required field TIME_SYSTEM is missing.\n";
throw UtilityException(errmsg);
}
if (timeSystem != "UTC")
{
std::string errmsg = segError;
errmsg += "Specified TIME_SYSTEM \"" + timeSystem;
errmsg += "\" is not supported at this time.\n";
throw UtilityException(errmsg);
}
// Start and stop times
if ((startTime == UNSET_REAL) || (stopTime == UNSET_REAL))
{
std::string errmsg = segError;
errmsg += "Required field START_TIME or STOP_TIME is missing.\n";
throw UtilityException(errmsg);
}
if (startTime > stopTime)
{
std::string errmsg = segError;
errmsg += "START_TIME must be less than STOP_TIME.\n";
throw UtilityException(errmsg);
}
// Usable start and stop times
if (((usableStartTime == UNSET_REAL) && (usableStopTime != UNSET_REAL)) ||
((usableStartTime != UNSET_REAL) && (usableStopTime == UNSET_REAL)))
{
std::string errmsg = segError;
errmsg += "If usable start and stop times are to be used, both ";
errmsg += "USEABLE_START_TIME and USEABLE_STOP_TIME must be present ";
errmsg += "in the segment.\n";
throw UtilityException(errmsg);
}
if ((usableStartTime != UNSET_REAL) &&
(usableStopTime != UNSET_REAL))
{
if ((usableStartTime < startTime) || (usableStartTime > stopTime))
{
std::string errmsg = segError;
errmsg += "USEABLE_START_TIME must be ";
errmsg += "between START_TIME and STOP_TIME.\n";
throw UtilityException(errmsg);
}
if ((usableStopTime < startTime) || (usableStopTime > stopTime))
{
std::string errmsg = segError;
errmsg += "USEABLE_STOP_TIME must be ";
errmsg += "between START_TIME and STOP_TIME.\n";
throw UtilityException(errmsg);
}
if (usableStartTime > usableStopTime)
{
std::string errmsg = segError;
errmsg += "USEABLE_START_TIME must be less than USEABLE_STOP_TIME.\n";
throw UtilityException(errmsg);
}
usesUsableTimes = true;
}
else
{
usesUsableTimes = false;
}
GetUsableIndexRange(firstUsable, lastUsable);
// Interpolation
if ((interpolationMethod == "LAGRANGE") || checkLagrangeOrder)
{
if ((interpolationDegree < 0) || (interpolationDegree > 9))
{
std::string errmsg = segError;
errmsg += "Field INTERPOLATION_DEGREE is out-of-range. For Lagrange ";
errmsg += "interpolation, degree must be between 0 and 9.\n";
throw UtilityException(errmsg);
}
}
// Is there data?
if (checkData && dataStore.empty())
{
std::string errmsg = segError;
errmsg += "File does not contain data for segment of ";
errmsg += "data type " + dataType + "\n";
throw UtilityException(errmsg);
}
#ifdef DEBUG_EM_VALIDATE
MessageInterface::ShowMessage("EXITing EMSegment::Validate ...\n");
#endif
return true;
}
