Improved the route vertex generating algorithm
Added basic segment joining