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.
Add test setting the matrix diagonal to 1.0