Algorithm Design For Safe And Efficient Societal-Scale Navigation