Merge of initial pathfinding routines
Refined pathfinding routines; initial multi-agent tests