Added diagonalization routine, switched to column major format