Create matrix_utils.c to reduce duplicated code