An underwater pathfinding algorithm for optimised planning of survey dives