Add more matrix concatenation features

[qhdwight]

I'm working on implementing Linear Model Predictive Control for a robotics project (https://github.com/qhdwight/mpc-rs). As I do that I am generating some ideas for ease of use functions that numpy/scipy have already. Specifically with regards to concatenating together matrices.

vstack/hstack: https://numpy.org/doc/stable/reference/generated/numpy.vstack.html

block_diag: https://docs.scipy.org/doc/scipy/reference/generated/scipy.linalg.block_diag.html

Here is a sample implementation:

fn diagonal_block_matrix<T: Scalar + Zero>(matrices: DVector<DMatrix<T>>) -> DMatrix<T> {
    let total_rows = matrices.iter().map(DMatrix::nrows).sum();
    let total_cols = matrices.iter().map(DMatrix::nrows).sum();
    let mut block_diagonal_matrix = DMatrix::zeros(total_rows, total_cols);
    // TODO: use ranges instead of two diff variables?
    let mut r = 0;
    let mut c = 0;
    for (i, matrix) in matrices.into_iter().enumerate() {
        let nr = r + matrix.nrows();
        let nc = c + matrix.ncols();
        block_diagonal_matrix.index_mut((r..nr, c..nc)).copy_from(&matrix);
        r = nr;
        c = nc;
    }
    block_diagonal_matrix
}

Bonus would be implementing the from trait for Matrix -> DMatrix, e.g. here is what I use right now:

fn into_dynamic<T, R, C, S>(matrix: Matrix<T, R, C, S>) -> DMatrix<T> where
    T: Scalar + Zero,
    R: Dim + DimName, C: Dim + DimName,
    S: RawStorage<T, R, C> {
    let mut dynamic_matrix = DMatrix::zeros(matrix.nrows(), matrix.ncols());
    dynamic_matrix.copy_from(&matrix);
    dynamic_matrix
}