AQdded intersection finding function, needs more unit tests. Added start point to waypoints to help UI implementors