Add a few optimized matrix functions