Modified cost function to route more efficiently