Add some new method to matrix and refactor some old