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