Better implementation of Dirichlet BC