Path finding (Even more optimized!)
Controls
Left cick - start point.
Right click - end point.
Middle mouse - new map.