Streamlined dof calculation
Removed copy of block_diag
Used flatten() for flattening the position arrays