New improved route finding algorithm