Waypoints fix

There were some problems with how the map lib was handling zero length line segments. This fixes #1075