Optimized matrix math