Refactor for computing mass matrix