//------------------------------------------------------------------------------
bool PropagationStateManager::BuildState()
{
#ifdef DEBUG_STATE_CONSTRUCTION
MessageInterface::ShowMessage("Entered BuildState()\n");
MessageInterface::ShowMessage("StateMap:\n");
for (Integer index = 0; index < stateSize; ++index)
{
MessageInterface::ShowMessage(" %s.%s",
stateMap[index]->objectName.c_str(),
stateMap[index]->elementName.c_str());
}
#endif
// Determine the size of the propagation state vector
stateSize = SortVector();
std::map<std::string,Integer> associateMap;
// Build the associate map
std::string name;
for (Integer index = 0; index < stateSize; ++index)
{
name = stateMap[index]->objectName;
if (associateMap.find(name) == associateMap.end())
associateMap[name] = index;
}
state.SetSize(stateSize);
for (Integer index = 0; index < stateSize; ++index)
{
name = stateMap[index]->objectName;
std::stringstream sel("");
sel << stateMap[index]->subelement;
state.SetElementProperties(index, stateMap[index]->elementID,
name + "." + stateMap[index]->elementName + "." + sel.str(),
associateMap[stateMap[index]->associateName]);
}
#ifdef DEBUG_STATE_CONSTRUCTION
MessageInterface::ShowMessage(
"Propagation state vector has %d elements:\n", stateSize);
StringArray props = state.GetElementDescriptions();
for (Integer index = 0; index < stateSize; ++index)
MessageInterface::ShowMessage(" %d: %s --> associate: %d\n", index,
props[index].c_str(), state.GetAssociateIndex(index));
#endif
#ifdef DUMP_STATE
MapObjectsToVector();
for (Integer i = 0; i < stateSize; ++i)
MessageInterface::ShowMessage("State[%02d] = %.12lf, %s %d\n", i, state[i],
"RefState start =", state.GetAssociateIndex(i));
#endif
return true;
}
/**
* Corrects the box position so as not to collide with
* the walls.
*/
Rect MovementMap::CorrectPosition(Rect box)
{
std::cout << "Pos:(" << previousPos.GetX() << "," << previousPos.GetY() << ")" << std::endl;
std::cout << "Width: " << previousPos.GetW() << std::endl;
std::cout << "Height: " << previousPos.GetH() << std::endl;
std::vector<Rect> collidingTiles;
while (IsColliding(box))
{
collidingTiles = GetCollidingTiles(box);
SortVector(collidingTiles);
for (int i = 0; i < (int)collidingTiles.size(); i++)
CorrectCollision(box, collidingTiles[i]);
collidingTiles.clear();
std::cout << "LOOP LULZ" << std::endl;
}
std::cout << "EXIT LOOP" << std::endl;
return box;
}
//------------------------------------------------------------------------------
bool EstimationStateManager::BuildState()
{
bool retval = false;
#ifdef DEBUG_STATE_CONSTRUCTION
MessageInterface::ShowMessage("Entered BuildState()\n");
#endif
// Determine the size of the propagation state vector
stateSize = SortVector();
if (stateSize <= 0)
{
std::stringstream sizeVal;
sizeVal << stateSize;
throw EstimatorException("Estimation state vector size is " +
sizeVal.str() + "; estimation is not possible.");
}
std::map<std::string,Integer> associateMap;
// Build the associate map
std::string name;
for (Integer index = 0; index < stateSize; ++index)
{
name = stateMap[index]->objectName;
if (associateMap.find(name) == associateMap.end())
associateMap[name] = index;
}
state.SetSize(stateSize);
// Build the data structures for the STM and covariance matrix
stm.SetSize(stateSize, stateSize);
covariance.SetDimension(stateSize);
stmColCount = stateSize;
covColCount = stateSize;
for (Integer index = 0; index < stateSize; ++index)
{
name = stateMap[index]->objectName;
std::stringstream sel("");
sel << stateMap[index]->subelement;
state.SetElementProperties(index, stateMap[index]->elementID,
name + "." + stateMap[index]->elementName + "." + sel.str(),
associateMap[name]);
}
// Initialize the STM matrix
for (Integer i = 0; i < stateSize; ++i)
for (Integer j = 0; j < stateSize; ++j)
if (i == j)
stm(i,j) = 1.0;
else
stm(i,j) = 0.0;
// Now build the covariance, using the elements the user has set and defaults
// for the rest
for (Integer i = 0; i < stateSize;)
{
Integer size = stateMap[i]->length;
Integer id = stateMap[i]->parameterID;
Covariance *cov = stateMap[i]->object->GetCovariance();
#ifdef DEBUG_COVARIANCE
MessageInterface::ShowMessage("Found length = %d for id = %d\n", size, id);
#endif
Rmatrix *subcov = cov->GetCovariance(id);
if (subcov)
{
#ifdef DEBUG_COVARIANCE
MessageInterface::ShowMessage("Found %d by %d subcovariance\n",
subcov->GetNumRows(), subcov->GetNumColumns());
#endif
for (Integer j = 0; j < size; ++j)
for (Integer k = 0; k < size; ++k)
covariance(i+j,i+k) = (*subcov)(j,k);
}
else
{
#ifdef DEBUG_COVARIANCE
MessageInterface::ShowMessage("Found no subcovariance\n");
#endif
if (stateMap[i]->elementName == "CartesianState")
{
for (Integer j = 0; j < size; ++j)
{
for (Integer k = 0; k < size; ++k)
{
if (j < 3)
covariance(i+j,i+k) = (j == k ? 1.0e12 : 0.0);
else
covariance(i+j,i+k) = (j == k ? 1.0e6 : 0.0);
}
}
}
else // Other defaults are set to the identity
{
for (Integer j = 0; j < size; ++j)
{
for (Integer k = 0; k < size; ++k)
{
covariance(i+j,i+k) = (j == k ? 1.0e0 : 0.0);
}
}
}
}
i += size;
}
#ifdef DEBUG_STATE_CONSTRUCTION
MessageInterface::ShowMessage(
"Estimation state vector has %d elements:\n", stateSize);
StringArray props = state.GetElementDescriptions();
for (Integer index = 0; index < stateSize; ++index)
MessageInterface::ShowMessage(" %d: %s\n", index,
props[index].c_str());
#endif
#ifdef DUMP_STATE
MapObjectsToVector();
for (Integer i = 0; i < stateSize; ++i)
MessageInterface::ShowMessage("State[%02d] = %.12lf, %s %d\n", i, state[i],
"RefState start =", state.GetAssociateIndex(i));
MessageInterface::ShowMessage("State Transition Matrix = %s\n",
stm.ToString().c_str());
MessageInterface::ShowMessage("Covariance Matrix = %s\n",
covariance.ToString().c_str());
#endif
return retval;
}
