Optimized matrix_init function