diff --git a/src/state_block/state_composite.cpp b/src/state_block/state_composite.cpp index 84a7a526a3f05b8d1728f07cc4fc08fc672589ad..3efebc717a2d8daad66ce7e2c8f879e9bc5faf29 100644 --- a/src/state_block/state_composite.cpp +++ b/src/state_block/state_composite.cpp @@ -69,15 +69,18 @@ wolf::MatrixComposite MatrixComposite::operator *(const MatrixComposite &_N) con { const auto &i = pair_i_Mi.first; const auto &Mi = pair_i_Mi.second; + for (const auto &pair_k_Nk : _N.matrix_composite_) { const auto &k = pair_k_Nk.first; const auto &Nk = pair_k_Nk.second; + for (const auto &pair_j_Nkj : Nk) { const auto &j = pair_j_Nkj.first; const auto &Nkj = pair_j_Nkj.second; const auto &Mik = Mi.at(k); + if (MN.count(i, j) == 0) MN.emplace(i, j, Mik * Nkj); else @@ -94,11 +97,13 @@ wolf::StateComposite MatrixComposite::operator *(const StateComposite &_x) const for (const auto &pair_rkey_row : matrix_composite_) { const auto &rkey = pair_rkey_row.first; - const auto &row = pair_rkey_row.second; + const auto &row = pair_rkey_row.second; + for (const auto &pair_ckey_mat : row) { - const auto &ckey = pair_ckey_mat.first; + const auto &ckey = pair_ckey_mat.first; const auto &J_r_c = pair_ckey_mat.second; + if (y.count(rkey)) y.at(rkey) += J_r_c * _x.at(ckey); else @@ -113,9 +118,11 @@ std::ostream& operator <<(std::ostream &_os, const MatrixComposite &_M) for (const auto &pair_row_cols : _M.matrix_composite_) { const auto row = pair_row_cols.first; + for (const auto &pair_col_blk : pair_row_cols.second) { const auto &col = pair_col_blk.first; + _os << "\n block(" << row << "," << col << ") = \n" << pair_col_blk.second; } } @@ -128,6 +135,7 @@ std::ostream& operator <<(std::ostream &_os, const StateComposite &_x) { const auto &row = pair_row_blk.first; const auto &vec = pair_row_blk.second; + _os << "\n block(" << row << ") = \n" << vec; } return _os;