Resolve "Problem::getState(structure) doesn't care about structure"

Closes #343 (closed)

include/core/state_block/has_state_blocks.h

@@ -74,12 +74,12 @@ class HasStateBlocks
         VectorComposite getState(const StateStructure& structure="") const;
         void setState(const VectorComposite& _state, const bool _notify = true);
         void setState(const Eigen::VectorXd& _state, const StateStructure& _structure, const std::list<SizeEigen>& _sizes, const bool _notify = true);
-        void setState(const Eigen::VectorXd& _state, const StateStructure& _sub_structure="", const bool _notify = true);
+        void setState(const Eigen::VectorXd& _state, const StateStructure& _structure="", const bool _notify = true);
         void setState(const StateStructure& _structure, const std::list<VectorXd>& _vectors, const bool _notify = true);
 
         VectorXd getStateVector(const StateStructure& structure="") const;
-        unsigned int getSize(const StateStructure& _sub_structure="") const;
-        unsigned int getLocalSize(const StateStructure& _sub_structure="") const;
+        unsigned int getSize(const StateStructure& _structure="") const;
+        unsigned int getLocalSize(const StateStructure& _structure="") const;
 
         // Perturb state
         void            perturb(double amplitude = 0.01);
@@ -225,13 +225,13 @@ inline void HasStateBlocks::setState(const VectorComposite& _state, const bool _
     }
 }
 
-inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateStructure& _sub_structure, const bool _notify)
+inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateStructure& _structure, const bool _notify)
 {
     StateStructure structure;
-    if (_sub_structure == "")
+    if (_structure == "")
         structure = structure_;
     else
-        structure = _sub_structure;
+        structure = _structure;
 
     int size = getSize(structure);
     assert(_state.size() == size && "In FrameBase::setState wrong state size");
@@ -286,14 +286,14 @@ inline void HasStateBlocks::setState(const StateStructure& _structure, const std
 }
 
 
-//// _sub_structure can be either stateblock structure of the node or a subset of this structure
-inline VectorXd HasStateBlocks::getStateVector(const StateStructure& _sub_structure) const
+//// _structure can be either stateblock structure of the node or a subset of this structure
+inline VectorXd HasStateBlocks::getStateVector(const StateStructure& _structure) const
 {
     StateStructure structure;
-    if (_sub_structure == "")
+    if (_structure == "")
         structure = structure_;
     else
-        structure = _sub_structure;
+        structure = _structure;
 
     VectorXd state(getSize(structure));
 
@@ -312,21 +312,31 @@ inline VectorXd HasStateBlocks::getStateVector(const StateStructure& _sub_struct
 
 inline VectorComposite HasStateBlocks::getState(const StateStructure& _structure) const
 {
+    const StateStructure& structure = (_structure == "" ? structure_ : _structure);
+
     VectorComposite state;
-    for (auto& pair_key_sb : state_block_map_)
+
+    for (const auto ckey : structure)
     {
-        state.emplace(pair_key_sb.first, pair_key_sb.second->getState());
+        const auto& key = string(1,ckey); // ckey is char
+
+        auto state_it = state_block_map_.find(key);
+
+        if (state_it != state_block_map_.end())
+
+            state.emplace(key, state_it->second->getState());
     }
+
+//    for (auto& pair_key_sb : state_block_map_)
+//    {
+//        state.emplace(pair_key_sb.first, pair_key_sb.second->getState());
+//    }
     return state;
 }
 
-inline unsigned int HasStateBlocks::getSize(const StateStructure& _sub_structure) const
+inline unsigned int HasStateBlocks::getSize(const StateStructure& _structure) const
 {
-    StateStructure structure;
-    if (_sub_structure == "")
-        structure = structure_;
-    else
-        structure = _sub_structure;
+    const StateStructure& structure = (_structure == "" ? structure_ : _structure);
 
     unsigned int size = 0;
     for (const char key : structure)
@@ -341,7 +351,6 @@ inline unsigned int HasStateBlocks::getSize(const StateStructure& _sub_structure
     return size;
 }
 
-//<<<<<<< HEAD
 inline std::unordered_map<std::string, StateBlockPtr>::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb) const
 {
     const auto& it = std::find_if(state_block_map_.begin(),
@@ -380,13 +389,13 @@ inline bool HasStateBlocks::stateBlockKey(const StateBlockPtr &_sb, std::string&
     }
 }
 
-inline unsigned int HasStateBlocks::getLocalSize(const StateStructure& _sub_structure) const
+inline unsigned int HasStateBlocks::getLocalSize(const StateStructure& _structure) const
 {
     StateStructure structure;
-    if (_sub_structure == "")
+    if (_structure == "")
         structure = structure_;
     else
-        structure = _sub_structure;
+        structure = _structure;
 
     unsigned int size = 0;
     for (const char key : structure)