WIP: Resolve "Complete state vector new data structure?"

Closes #287 (closed)

Continues in !358 (closed)

include/core/state_block/state_block.h

@@ -146,6 +146,9 @@ public:
          **/
         void resetLocalParamUpdated();
 
+        virtual void setIdentity(bool _notify = true);
+        virtual void setZero    (bool _notify = true);
+
         /** \brief perturb state
          */
         void perturb(double amplitude = 0.1);
@@ -286,6 +289,17 @@ inline double* StateBlock::getStateData()
     return state_.data();
 }
 
+inline void StateBlock::setIdentity(bool _notify)
+{
+    setState( VectorXd::Zero(state_size_), _notify );
+}
+
+inline void StateBlock::setZero(bool _notify)
+{
+    setIdentity(_notify);
+}
+
+
 }// namespace wolf
 
 #endif