Updated pathfinding
Added goal oriented path finding
Merge remote-tracking branch 'origin/master'