//---------------------------------------------------------------------------
int SetState(double epoch, double state[], int stateDim)
{
int retval = -1;
if (pSetup != NULL)
{
GmatState *theState = pSetup->GetPropStateManager()->GetState();
if (stateDim <= theState->GetSize())
{
theState->SetEpoch(epoch);
theState->SetState(state, stateDim);
retval = 0;
}
else
{
char msg[128];
sprintf(msg, "ERROR: Incoming state size (%d) is larger than the "
"propagation state vector size (%d)!\n Epoch: %lf\n "
"State = [%lf %lf %lf %lf %lf %lf]\n", stateDim,
theState->GetSize(), epoch, state[0], state[1], state[2],
state[3], state[4], state[5]);
lastMsg = msg;
retval = -2;
}
}
else
lastMsg = "ERROR in SetState: The propagation setup is not yet set.";
return retval;
}
//------------------------------------------------------------------------------
void Formation::UpdateElements()
{
Integer size, index = 0;
GmatState *ps;
for (std::vector<SpaceObject*>::iterator i = components.begin();
i != components.end(); ++i)
{
ps = &((*i)->GetState());
size = ps->GetSize();
memcpy(ps->GetState(), &((state.GetState())[index]), size*sizeof(Real));
index += size;
if ((*i)->GetType() == Gmat::FORMATION)
((Formation*)(*i))->UpdateElements();
}
}
//------------------------------------------------------------------------------
void Formation::UpdateState()
{
Integer size, index = 0;
GmatState *ps;
Real ep0 = 0.0, ep;
for (std::vector<SpaceObject*>::iterator i = components.begin();
i != components.end(); ++i)
{
if (i == components.begin())
{
ep0 = (*i)->GetEpoch();
#ifdef DEBUG_FORMATION
MessageInterface::ShowMessage("Base epoch is %.12lf\n", ep0);
#endif
}
else
{
ep = (*i)->GetEpoch();
if (ep != ep0)
MessageInterface::ShowMessage(
"WARNING! Formation Member Epochs are not synchronized!\n"
"First spacecraft epoch is %.12lf, but %s has epoch %.12lf\n",
ep0, (*i)->GetName().c_str(), ep);
}
ps = &((*i)->GetState());
size = ps->GetSize();
#ifdef DEBUG_FORMATION_UPDATES
MessageInterface::ShowMessage(
"Formation: Updating(%d to %d) from %s::%s\n",
index, index + size - 1, instanceName.c_str(), (*i)->GetName().c_str());
#endif
memcpy(&((state.GetState())[index]), ps->GetState(), size*sizeof(Real));
index += size;
if ((*i)->GetType() == Gmat::FORMATION)
((Formation*)(*i))->UpdateState();
}
SetEpoch(ep0);
}
//------------------------------------------------------------------------------
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);
}
//------------------------------------------------------------------------------
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;
}
