Fix setting matrix diagonal if row DataSet has dim > 1

If the row DataSet has dimension > 1 we need to treat the given rows as block
indices and set the diagonals of all rows in each block.