Speed-ups: avoid re-allocation of temporary arrays, optimize add_vec_to_mat