Refactor matrix classes and functions

Refactor Matrix classes and functions to use output arguments instead of RCP.