Resolve "New data structure for storing stateblocks"

Closes #256 (closed)

include/core/state_block/has_state_blocks.h

+/**
+ * \file has_state_blocks.h
+ *
+ *  Created on: Aug 27, 2019
+ *      \author: jsola
+ */
+
+#ifndef STATE_BLOCK_HAS_STATE_BLOCKS_H_
+#define STATE_BLOCK_HAS_STATE_BLOCKS_H_
+
+#include "core/common/wolf.h"
+#include "core/state_block/state_block.h"
+
+#include <map>
+
+namespace wolf
+{
+
+class HasStateBlocks
+{
+    public:
+        HasStateBlocks();
+        virtual ~HasStateBlocks();
+
+        const std::map<std::string, StateBlockPtr>& getStateBlockMap() const;
+        std::map<std::string, StateBlockPtr>& getStateBlockMap();
+
+    public:
+        // Some typical shortcuts -- not all should be coded here, see notes below.
+        StateBlockPtr getP() const;
+        StateBlockPtr getO() const;
+        StateBlockPtr getV() const; // This probably not here but in derived classes only
+        StateBlockPtr getW() const; // This probably not here but in derived classes only
+        StateBlockPtr getIntrinsics() const; // This probably not here but in derived classes only
+        void setP(const StateBlockPtr _p_ptr);
+        void setO(const StateBlockPtr _o_ptr);
+        void setV(const StateBlockPtr _v_ptr); // This probably not here but in derived classes only
+        void setW(const StateBlockPtr _w_ptr); // This probably not here but in derived classes only
+        void setIntrinsics(const StateBlockPtr _i_ptr); // This probably not here but in derived classes only
+
+    public:
+        // These act on all state blocks. Per-block action must be done through state_block.fix() or through extended API in derived classes of this.
+        void fix();
+        void unfix();
+        bool isFixed() const;
+
+    protected:
+        StateBlockPtr setStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb);
+        StateBlockPtr getStateBlock(const std::string& _sb_type) const;
+
+        // Emplace derived state blocks (angle, quaternion, etc).
+        template<typename SB, typename ... Args>
+        std::shared_ptr<SB> emplaceStateBlock(const std::string& _sb_type, Args&&... _args_of_derived_state_block_constructor);
+
+        // Emplace base state blocks.
+        template<typename ... Args>
+        inline std::__1::shared_ptr<StateBlock> emplaceStateBlock(const std::string& _sb_type, Args&&... _args_of_base_state_block_constructor);
+
+
+    private:
+        std::map<std::string, StateBlockPtr> state_block_map_;
+
+};
+
+inline HasStateBlocks::HasStateBlocks() :
+        state_block_map_()
+{
+    //
+}
+
+inline HasStateBlocks::~HasStateBlocks()
+{
+    //
+}
+
+inline std::map<std::string, StateBlockPtr>& HasStateBlocks::getStateBlockMap()
+{
+    return state_block_map_;
+}
+
+inline const std::map<std::string, StateBlockPtr>& HasStateBlocks::getStateBlockMap() const
+{
+    return state_block_map_;
+}
+
+inline wolf::StateBlockPtr HasStateBlocks::setStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb)
+{
+    assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type!");
+    state_block_map_.emplace(_sb_type, _sb);
+    return _sb;
+}
+
+template<typename SB, typename ... Args>
+inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const std::string& _sb_type, 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!");
+    std::shared_ptr<SB> sb(std::forward<Args>(_args_of_derived_state_block_constructor)...);
+    state_block_map_.emplace(_sb_type, sb);
+    return sb;
+}
+
+template<typename ... Args>
+inline StateBlockPtr HasStateBlocks::emplaceStateBlock<StateBlock>(const std::string& _sb_type, 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!");
+    std::shared_ptr<StateBlock> sb(std::forward<Args>(_args_of_base_state_block_constructor)...);
+    state_block_map_.emplace(_sb_type, sb);
+    return sb;
+}
+
+inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const std::string& _sb_type) const
+{
+    if (state_block_map_.count(_sb_type))
+        return state_block_map_.at(_sb_type);
+    else
+        return nullptr;
+}
+
+inline wolf::StateBlockPtr HasStateBlocks::getP() const
+{
+    return getStateBlock("P");
+}
+
+inline wolf::StateBlockPtr HasStateBlocks::getO() const
+{
+    return getStateBlock("O");
+}
+
+inline wolf::StateBlockPtr HasStateBlocks::getV() const
+{
+    return getStateBlock("V");
+}
+
+inline wolf::StateBlockPtr HasStateBlocks::getW() const
+{
+    return getStateBlock("W");
+}
+
+inline wolf::StateBlockPtr HasStateBlocks::getIntrinsics() const
+{
+    return getStateBlock("I");
+}
+
+inline void HasStateBlocks::setP(const StateBlockPtr _p_ptr)
+{
+    setStateBlock("P", _p_ptr);
+}
+
+inline void HasStateBlocks::setO(const StateBlockPtr _o_ptr)
+{
+    setStateBlock("O", _o_ptr);
+}
+
+inline void HasStateBlocks::setV(const StateBlockPtr _v_ptr)
+{
+    setStateBlock("V", _v_ptr);
+}
+
+inline void HasStateBlocks::setW(const StateBlockPtr _w_ptr)
+{
+    setStateBlock("W", _w_ptr);
+}
+
+inline void HasStateBlocks::setIntrinsics(const StateBlockPtr _i_ptr)
+{
+    setStateBlock("I", _i_ptr);
+}
+
+inline void HasStateBlocks::fix()
+{
+    for (auto sbp : state_block_map_)
+        if (sbp.second != nullptr)
+            sbp.second->fix();
+}
+
+inline void HasStateBlocks::unfix()
+{
+    for (auto sbp : state_block_map_)
+        if (sbp != nullptr)
+            sbp->unfix();
+}
+
+inline bool HasStateBlocks::isFixed() const
+{
+    bool fixed = true;
+    for (auto sb : state_block_map_)
+    {
+        if (sb.second)
+            fixed &= sb.second->isFixed();
+    }
+    return fixed;
+}
+
+} // namespace wolf
+#endif /* STATE_BLOCK_HAS_STATE_BLOCKS_H_ */