//------------------------------------------------------------------------------
Real Formation::SetRealParameter(const Integer id, const Real value)
{
Real retval = -1.0;
if ((id >= FORMATION_CARTESIAN_STATE) &&
(id < FORMATION_CARTESIAN_STATE + dimension))
{
state[id - FORMATION_CARTESIAN_STATE] = value;
retval = value;
}
else
{
retval = FormationInterface::SetRealParameter(id, value);
if (id == EPOCH_PARAM)
{
if (retval != value)
throw SpaceObjectException(
"Formation update returned incorrect epoch");
// Update the epoch on the constituent pieces
for (std::vector<SpaceObject*>::iterator i = components.begin();
i != components.end(); ++i)
{
retval = (*i)->SetRealParameter(id, value);
if (retval != value)
throw SpaceObjectException(
"Formation constituent returned incorrect epoch");
}
}
}
return retval;
}
//------------------------------------------------------------------------------
bool Formation::SetRefObject(GmatBase *obj, const Gmat::ObjectType type,
const std::string &name)
{
SpaceObject *so;
if (type == Gmat::SPACECRAFT)
{
so = ((SpaceObject*)(obj));
if (find(components.begin(), components.end(), so) == components.end())
{
GmatState *ps = &(so->GetState());
Integer size = ps->GetSize();
dimension += size;
Real newepoch = so->GetEpoch();
if (components.size() == 0)
state.SetEpoch(newepoch);
else
if (state.GetEpoch() != newepoch)
{
char errorMsg[256];
sprintf(errorMsg, "Epochs (%lf) and (%lf) are not synchronized "
"in the formation %s", newepoch, state.GetEpoch(),
instanceName.c_str());
throw SpaceObjectException(errorMsg);
}
components.push_back(so);
}
if (type == Gmat::FORMATION)
{
throw SpaceObjectException("GMAT does not allow Formations of "
"Formations, so the Formation \"" + name + "\" cannot be added "
"to the Formation \"" + instanceName + "\".");
}
return true;
}
return FormationInterface::SetRefObject(obj, type, name);
}
//------------------------------------------------------------------------------
const Rvector6 SpaceObject::GetMJ2000State(const A1Mjd &atTime)
{
#ifdef DEBUG_J2000_STATE
MessageInterface::ShowMessage(
"SpaceObject::GetMJ2000State entered; epoch is %lf\n", atTime.Get());
#endif
if (j2000Body == NULL)
{
std::string errmsg = "SpaceObject::GetMJ2000State J2000 body ";
errmsg += j2000BodyName + " not yet set for ";
errmsg += instanceName;
errmsg += ". Possible circular dependency in Spacecraft and ";
errmsg += "Coordinate System configuration.\n";
throw SpaceObjectException(errmsg);
}
Real *st = state.GetState(); //ps.GetState();
#ifdef DEBUG_J2000_STATE
MessageInterface::ShowMessage(" %s Object state: [%lf %lf %lf %lf %lf "
"%lf]\n", instanceName.c_str(), st[0], st[1], st[2], st[3], st[4],
st[5]);
MessageInterface::ShowMessage(" Accessing J2000 body state for %s\n",
j2000Body->GetName().c_str());
#endif
Rvector6 bodyState = j2000Body->GetMJ2000State(atTime);
#ifdef DEBUG_J2000_STATE
MessageInterface::ShowMessage(" MJ2000: [%lf %lf %lf %lf %lf %lf]\n",
bodyState[0], bodyState[1], bodyState[2], bodyState[3], bodyState[4],
bodyState[5]);
#endif
Rvector6 j2kState;
j2kState[0] = st[0] - bodyState[0];
j2kState[1] = st[1] - bodyState[1];
j2kState[2] = st[2] - bodyState[2];
j2kState[3] = st[3] - bodyState[3];
j2kState[4] = st[4] - bodyState[4];
j2kState[5] = st[5] - bodyState[5];
#ifdef DEBUG_J2000_STATE
MessageInterface::ShowMessage(" J2K state: [%lf %lf %lf %lf %lf %lf]\n",
j2kState[0], j2kState[1], j2kState[2], j2kState[3], j2kState[4],
j2kState[5]);
#endif
return j2kState;
}
//------------------------------------------------------------------------------
const Rvector6 Formation::GetMJ2000State(const A1Mjd &atTime)
{
satCount = components.size();
if (j2000Body == NULL)
throw SpaceObjectException("MJ2000 body not yet set for " +
instanceName);
// First calculate the geometric center of the formation
Rvector6 centerState;
GmatState ps = GetState();
Real *st = ps.GetState();
if (satCount == 0)
{
MessageInterface::ShowMessage(
"Warning: Attempting to find MJ2000 state for an empty formation\n");
return centerState;
}
// The Formation GmatState contains state data for the spacecraft, tanks, and
// (eventually) attitude. The first 6*satcount elements are the spacecraft
// position and velocity data.
for (UnsignedInt i = 0; i < satCount; ++i)
{
for (Integer j = 0; j < 6; ++j)
centerState[j] += st[i*6+j];
}
centerState /= satCount;
// Then calculate the J2000 data based on that state
Rvector6 bodyState = j2000Body->GetMJ2000State(atTime);
// If origin is NULL, assume it is set at the J2000 origin.
if (origin)
{
Rvector6 offset = origin->GetMJ2000State(atTime);
bodyState -= offset;
}
return centerState - bodyState;
}
//------------------------------------------------------------------------------
void Formation::BuildState()
{
#ifdef DEBUG_FORMATION_ACTIONS
MessageInterface::ShowMessage("%s%s%s%d\n",
"In BuildState, Formation \"", instanceName.c_str(),
"\" has dimension ", dimension);
#endif
if (dimension <= 0)
throw SpaceObjectException(
"Error building Formation state; no spacecraft are set");
// Setup the GmatState
Real *data = new Real[dimension], *st;
Integer j = 0, k;
GmatState *ps;
if (state.GetSize() < dimension)
state.SetSize(dimension);
for (std::vector<SpaceObject*>::iterator i = components.begin();
i != components.end(); ++i)
{
if ((*i) == NULL)
throw SpaceObjectException(
"Error building Formation state; member spacecraft not set");
ps = &((*i)->GetState());
st = ps->GetState();
for (k = 0; k < ps->GetSize(); ++k)
{
data[j + k] = st[k];
}
j += ps->GetSize();
}
#ifdef DEBUG_FORMATION_ACTIONS
MessageInterface::ShowMessage("%s%s%s\n",
"In BuildState, Formation \"", instanceName.c_str(),
"\" consists of these spacecraft names:");
for (StringArray::iterator i = componentNames.begin();
i != componentNames.end(); ++i)
MessageInterface::ShowMessage(" \"%s\"\n", i->c_str());
MessageInterface::ShowMessage("%s%s%s\n",
"In BuildState, Formation \"", instanceName.c_str(),
"\" consists of these spacecraft:");
for (std::vector<SpaceObject *>::iterator j = components.begin();
j < components.end(); ++j)
{
MessageInterface::ShowMessage(" \"%s\"\n", (*j)->GetName().c_str());
}
#endif
if (!state.SetState(data, dimension))
{
delete [] data;
throw SpaceObjectException("Error building Formation state");
}
// as per kw report
// Shouldn't we delete data here since GmatState::SetState() copies the data
delete [] data;
}
