From 35019c35a2cfbd728f9e65f5fe72d13578686d27 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= <mfourmy@laas.fr> Date: Tue, 17 Mar 2020 13:51:00 +0100 Subject: [PATCH] Simplification of has_stateblock changes --- include/core/state_block/has_state_blocks.h | 68 ++++++++------------- 1 file changed, 25 insertions(+), 43 deletions(-) diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h index a93cf68a1..42b8b03a4 100644 --- a/include/core/state_block/has_state_blocks.h +++ b/include/core/state_block/has_state_blocks.h @@ -64,15 +64,11 @@ class HasStateBlocks void removeStateBlocks(ProblemPtr _problem); // States - virtual void setState(const Eigen::VectorXd& _state, const bool _notify = true); - void getState(std::string structure, Eigen::VectorXd& _state) const; - Eigen::VectorXd getState(std::string structure) const; - Eigen::VectorXd getState() const; - void getState(Eigen::VectorXd& _state) const; - unsigned int getSize() const; - unsigned int getSize(std::string _sub_structure) const; - unsigned int getLocalSize() const; - unsigned int getLocalSize(std::string _sub_structure) const; + inline void setState(const Eigen::VectorXd& _state, std::string _sub_structure="", const bool _notify = true); + void getState(Eigen::VectorXd& _state, std::string structure="") const; + Eigen::VectorXd getState(std::string structure="") const; + unsigned int getSize(std::string _sub_structure="") const; + unsigned int getLocalSize(std::string _sub_structure="") const; private: std::string structure_; @@ -219,17 +215,21 @@ inline bool HasStateBlocks::isFixed() const return fixed; } -inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const bool _notify) +inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, std::string _sub_structure, const bool _notify) { - int size = getSize(); - + if (_sub_structure == ""){ + _sub_structure = structure_; + } + int size = getSize(_sub_structure); assert(_state.size() == size && "In FrameBase::setState wrong state size"); unsigned int index = 0; - for (const char key : getStructure()) + for (const char key : _sub_structure) { const auto& sb = getStateBlock(key); - + if (!sb){ + WOLF_ERROR("Stateblock key ", key, " not in the structure"); + } sb->setState(_state.segment(index, sb->getSize()), _notify); // do not notify if state block is not estimated by the solver index += sb->getSize(); } @@ -237,20 +237,20 @@ inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const bool _ } // _sub_structure can be either stateblock structure of the node or a subset of this structure -inline void HasStateBlocks::getState(std::string _sub_structure, Eigen::VectorXd& _state) const +inline void HasStateBlocks::getState(Eigen::VectorXd& _state, std::string _sub_structure) const { - + if (_sub_structure == ""){ + _sub_structure = structure_; + } _state.resize(getSize(_sub_structure)); unsigned int index = 0; for (const char key : _sub_structure) { const auto& sb = getStateBlock(key); - if (!sb){ WOLF_ERROR("Stateblock key ", key, " not in the structure"); } - _state.segment(index,sb->getSize()) = sb->getState(); index += sb->getSize(); } @@ -261,33 +261,17 @@ inline Eigen::VectorXd HasStateBlocks::getState(std::string _sub_structure) cons { Eigen::VectorXd state; - getState(_sub_structure, state); + getState(state, _sub_structure); return state; } -inline void HasStateBlocks::getState(Eigen::VectorXd& _state) const -{ - getState(getStructure(), _state); -} - -inline Eigen::VectorXd HasStateBlocks::getState() const -{ - Eigen::VectorXd state; - - getState(state); - - return state; -} - - -inline unsigned int HasStateBlocks::getSize() const -{ - return getSize(structure_); -} inline unsigned int HasStateBlocks::getSize(std::string _sub_structure) const { + if (_sub_structure == ""){ + _sub_structure = structure_; + } unsigned int size = 0; for (const char key : _sub_structure) { @@ -301,13 +285,11 @@ inline unsigned int HasStateBlocks::getSize(std::string _sub_structure) const return size; } -inline unsigned int HasStateBlocks::getLocalSize() const -{ - return getLocalSize(structure_); -} - inline unsigned int HasStateBlocks::getLocalSize(std::string _sub_structure) const { + if (_sub_structure == ""){ + _sub_structure = structure_; + } unsigned int size = 0; for (const char key : _sub_structure) { -- GitLab