diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index 843e48b46d54261b315d0dfbafc95e2ac31555b9..5d660136a2081a9787087179017fb278f557969f 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -21,15 +21,15 @@ class HasStateBlocks
{
public:
HasStateBlocks();
- HasStateBlocks(const StateStructure& _structure) : structure_(_structure), state_block_map_() {}
+ HasStateBlocks(const StateStructure& _structure) : structure_(_structure), state_block_composite_() {}
virtual ~HasStateBlocks();
const StateStructure& getStructure() const { return structure_; }
void appendToStructure(const std::string& _frame_type){ structure_ += _frame_type; }
bool isInStructure(const std::string& _sb_type) { return structure_.find(_sb_type) != std::string::npos; }
- const StateBlockMap& getStateBlockMap() const;
- std::vector<StateBlockPtr> getStateBlockVec() const;
+ const StateBlockMap& getStateBlockMap() const;
+ std::vector<StateBlockPtr> getStateBlockVec() const;
// Some typical shortcuts -- not all should be coded here, see notes below.
StateBlockPtr getP() const;
@@ -44,7 +44,7 @@ class HasStateBlocks
virtual StateBlockPtr addStateBlock(const char _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem) { return addStateBlock(std::string(1,_sb_type), _sb, _problem); }
virtual unsigned int removeStateBlock(const std::string& _sb_type);
virtual unsigned int removeStateBlock(const char _sb_type);
- bool hasStateBlock(const std::string& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
+ bool hasStateBlock(const std::string& _sb_type) const { return state_block_composite_.has(_sb_type); }
bool hasStateBlock(const char _sb_type) const { return hasStateBlock(std::string(1, _sb_type)); }
bool hasStateBlock(const StateBlockPtr& _sb) const;
StateBlockPtr getStateBlock(const std::string& _sb_type) const;
@@ -77,6 +77,9 @@ class HasStateBlocks
bool getVectorComposite(VectorComposite& _state) const;
void setVectorComposite(const VectorComposite& _state);
+ StateBlockComposite& getStateBlockComposite();
+ const StateBlockComposite& getStateBlockComposite() const;
+
// Perturb state with noise
void perturb(double amplitude = 0.01);
@@ -85,8 +88,8 @@ class HasStateBlocks
private:
- StateStructure structure_; // TBR
- StateBlockMap state_block_map_;
+ StateStructure structure_; // TODO: remove?
+ StateBlockComposite state_block_composite_;
};
@@ -101,7 +104,8 @@ namespace wolf{
inline HasStateBlocks::HasStateBlocks() :
structure_(),
- state_block_map_()
+// state_block_map_(),
+ state_block_composite_()
{
//
}
@@ -113,7 +117,7 @@ inline HasStateBlocks::~HasStateBlocks()
inline const StateBlockMap& HasStateBlocks::getStateBlockMap() const
{
- return state_block_map_;
+ return state_block_composite_.getStateBlockMap();
}
inline std::vector<StateBlockPtr> HasStateBlocks::getStateBlockVec() const
@@ -133,13 +137,15 @@ inline unsigned int HasStateBlocks::removeStateBlock(const char _sb_type)
inline unsigned int HasStateBlocks::removeStateBlock(const std::string& _sb_type)
{
- return state_block_map_.erase(_sb_type);
+ return state_block_composite_.remove(_sb_type);
}
template<typename SB, typename ... Args>
inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor)
{
- assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+ assert(state_block_composite_.has(_sb_type) == false && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+
+// assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
std::shared_ptr<SB> sb = std::make_shared<SB>(std::forward<Args>(_args_of_derived_state_block_constructor)...);
addStateBlock(_sb_type, sb, _problem);
@@ -150,7 +156,8 @@ inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const std::string&
template<typename ... Args>
inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor)
{
- assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+ assert(state_block_composite_.has(_sb_type) == false && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+// assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
auto sb = std::make_shared<StateBlock>(std::forward<Args>(_args_of_base_state_block_constructor)...);
addStateBlock(_sb_type, sb, _problem);
@@ -161,18 +168,14 @@ inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const std::string& _sb_ty
inline bool HasStateBlocks::setStateBlock(const std::string _sb_type, const StateBlockPtr& _sb)
{
assert (structure_.find(_sb_type) != std::string::npos && "Cannot set a state block out of the state structure! Use addStateBlock instead.");
- assert ( state_block_map_.count(_sb_type) > 0 && "Cannot set an inexistent state block! Use addStateBlock instead.");
- state_block_map_.at(_sb_type) = _sb;
+ assert ( state_block_composite_.has(_sb_type) && "Cannot set an inexistent state block! Use addStateBlock instead.");
+ state_block_composite_.set(_sb_type, _sb);
return true; // success
}
inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const std::string& _sb_type) const
{
- auto it = state_block_map_.find(_sb_type);
- if (it != state_block_map_.end())
- return it->second;
- else
- return nullptr;
+ return state_block_composite_.at(_sb_type);
}
inline wolf::StateBlockPtr HasStateBlocks::getP() const
@@ -187,27 +190,17 @@ inline wolf::StateBlockPtr HasStateBlocks::getO() const
inline void HasStateBlocks::fix()
{
- for (auto pair_key_sbp : state_block_map_)
- if (pair_key_sbp.second != nullptr)
- pair_key_sbp.second->fix();
+ state_block_composite_.fix();
}
inline void HasStateBlocks::unfix()
{
- for (auto pair_key_sbp : state_block_map_)
- if (pair_key_sbp.second != nullptr)
- pair_key_sbp.second->unfix();
+ state_block_composite_.unfix();
}
inline bool HasStateBlocks::isFixed() const
{
- bool fixed = true;
- for (auto pair_key_sbp : state_block_map_)
- {
- if (pair_key_sbp.second)
- fixed &= pair_key_sbp.second->isFixed();
- }
- return fixed;
+ return state_block_composite_.isFixed();
}
inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateStructure& _sub_structure, const bool _notify)
@@ -268,40 +261,17 @@ inline Eigen::VectorXd HasStateBlocks::getState(const StateStructure& _sub_struc
inline StateBlockMap::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb) const
{
- const auto& it = std::find_if(state_block_map_.begin(),
- state_block_map_.end(),
- [_sb](const std::pair<std::string, StateBlockPtr>& pair)
- {
- return pair.second == _sb;
- }
- );
-
- return it;
+ return state_block_composite_.find(_sb);
}
inline bool HasStateBlocks::hasStateBlock(const StateBlockPtr &_sb) const
{
- const auto& it = this->find(_sb);
-
- return it != state_block_map_.end();
+ return state_block_composite_.has(_sb);
}
inline bool HasStateBlocks::stateBlockKey(const StateBlockPtr &_sb, std::string& _key) const
{
- const auto& it = this->find(_sb);
-
- bool found = (it != state_block_map_.end());
-
- if (found)
- {
- _key = it->first;
- return true;
- }
- else
- {
- _key = "";
- return false;
- }
+ return state_block_composite_.key(_sb, _key);
}
inline unsigned int HasStateBlocks::getSize(const StateStructure& _sub_structure) const
@@ -344,6 +314,16 @@ inline unsigned int HasStateBlocks::getLocalSize(const StateStructure& _sub_stru
return size;
}
+inline StateBlockComposite& HasStateBlocks::getStateBlockComposite()
+{
+ return state_block_composite_;
+}
+
+inline const StateBlockComposite& HasStateBlocks::getStateBlockComposite() const
+{
+ return state_block_composite_;
+}
+
} // namespace wolf
#endif /* STATE_BLOCK_HAS_STATE_BLOCKS_H_ */
diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
index 2c88b529cac09eaa7b3b65405d60cf417bacb416..2248a7d467979b2b86e3eff01e84b96ab894a577 100644
--- a/src/state_block/has_state_blocks.cpp
+++ b/src/state_block/has_state_blocks.cpp
@@ -6,8 +6,12 @@ namespace wolf
StateBlockPtr HasStateBlocks::addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem)
{
- assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
- state_block_map_.emplace(_sb_type, _sb);
+ assert(state_block_composite_.has(_sb) == false && "Trying to add a state block that already exists!");
+
+ assert(state_block_composite_.has(_sb_type) == false && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+
+ state_block_composite_.add(_sb_type, _sb);
+
if (!isInStructure(_sb_type))
appendToStructure(_sb_type);
@@ -46,7 +50,7 @@ void HasStateBlocks::removeStateBlocks(ProblemPtr _problem)
bool HasStateBlocks::getVectorComposite(VectorComposite &_state) const
{
- for (auto &pair_key_sb : state_block_map_)
+ for (auto &pair_key_sb : state_block_composite_.getStateBlockMap())
{
_state.emplace(pair_key_sb.first, pair_key_sb.second->getState());
}
@@ -64,13 +68,16 @@ void HasStateBlocks::setVectorComposite(const VectorComposite &_state)
{
for (const auto &pair_key_sb : _state)
{
- state_block_map_[pair_key_sb.first]->setState(pair_key_sb.second);
+ const auto& sb = state_block_composite_.at(pair_key_sb.first);
+
+ if(sb)
+ sb->setState(pair_key_sb.second);
}
}
void HasStateBlocks::perturb(double amplitude)
{
- for (const auto& pair_key_sb : state_block_map_)
+ for (const auto& pair_key_sb : state_block_composite_.getStateBlockMap())
{
auto& sb = pair_key_sb.second;
if (!sb->isFixed())
@@ -80,7 +87,7 @@ void HasStateBlocks::perturb(double amplitude)
void HasStateBlocks::plus(const VectorComposite& _dx)
{
- for (const auto& pair_key_sb : state_block_map_)
+ for (const auto& pair_key_sb : state_block_composite_.getStateBlockMap())
{
auto& sb = pair_key_sb.second;
auto& dv = _dx.at(pair_key_sb.first);