示例#1
0
//------------------------------------------------------------------------------
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;
}
示例#2
0
/**
 * 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;
}
示例#3
0
//------------------------------------------------------------------------------
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;
}