Add functions to util2d to make it more generic and versatile
Add tests for new util2d functionality