Implement QuadtreeMatrix, misc refactoring