[Added] Path finding algorithm

[dom-techwriter]

What

This algorithm will help us later calculate how to get to a certain point in the map.

We can later iterate on the current functionality to accept the start and end parameters, perhaps simplifying some thing as well.