Optimized matrix functions

Issue #1352