public override DIRECTION translateMove(NodeL from, NodeL to, ref View v)
{
int second = to.x - from.x;
int first = to.y - from.y;
if (first > 0) // dolu
{
return(DIRECTION.DIRECTION_FORWARD);
}
else if (first < 0) // nahoru
{
v = new TopView();
return(DIRECTION.DIRECTION_BACKWARD);
}
else if (second > 0) // doprava
{
v = new RightView();
return(DIRECTION.DIRECTION_LEFT);
}
else if (second < 0) // doleva
{
v = new LeftView();
return(DIRECTION.DIRECTION_RIGHT);
}
else
{
return(DIRECTION.WAIT);
}
}
/// <summary>
/// Reconstructs found path.
/// </summary>
/// <param name="cameFrom"></param>
/// <param name="current"></param>
/// <returns></returns>
Path recontructPath(Dictionary <NodeL, NodeL> cameFrom, NodeL current, /*List<Constraint> c,*/ int id)
{
Path p = new Path();
p.path.Add(current);
while (cameFrom.ContainsKey(current))
{
current = cameFrom[current];
p.path.Add(current);
}
p.path.Reverse();
/*for (int i = 0; i < p.path.Count; i++)
* {
* if (c.Find(item => item.a.ID == id && item.timeStep == i && item.nodeId == p.path[i].id) != null)
* {
* p.path.Insert(i, p.path[i - 1]);
* i++;
* }
* }*/
return(p);
}
public NodeL(int id, int x, int y, int time, IEnumerable <NodeL> neighbours, NodeL previous)
{
this.id = id;
this.x = x;
this.y = y;
this.time = time;
this.previous = previous;
this.neighbours = new List <NodeL>(neighbours);
}
/// <summary>
/// Finds node in opened with the lowest fScore value.
/// </summary>
/// <param name="fScore"></param>
/// <param name="opened"></param>
/// <returns></returns>
NodeL findMin(Dictionary <NodeL, int> fScore, HashSet <NodeL> opened)
{
int min = int.MaxValue;
NodeL minIndex = null;
foreach (NodeL n in fScore.Keys)
{
if (opened.Contains(n) && fScore[n] < min)
{
min = fScore[n];
minIndex = n;
}
}
return(minIndex);
}
public static View detectView(NodeL from, NodeL to)
{
int first = to.x - from.x;
int second = to.y - from.y;
if (first > 0)
{
return(new DownView());
}
else if (first < 0)
{
return(new TopView());
}
else if (second > 0)
{
return(new RightView());
}
else
{
return(new LeftView());
}
}
/// <summary>
/// Loads map and start and goal positions from given file.
/// </summary>
/// <param name="file"></param>
void LoadMap(string file)
{
StreamReader sr = new StreamReader(file);
string line = sr.ReadLine();
int width, height;
parseLine(line, out width, out height);
int size = width * height;
grid = new NodeL[size];
// for simplicity: initialize all fields of the grid - if any field is not available, than it will have no neighbours and it is unreachable
for (int i = 0; i < size; i++)
{
grid[i] = new NodeL(i, i % width, i / width);
}
// add to all nodes their neighbours
while ((line = sr.ReadLine()) != "X")
{
int firstNode, secondNode;
parseLine(line, out firstNode, out secondNode);
grid[firstNode].neighbours.Add(grid[secondNode]);
grid[secondNode].neighbours.Add(grid[firstNode]);
}
// creates all agents
List <Agent> agents = new List <Agent>();
while ((line = sr.ReadLine()) != null)
{
int start, goal;
parseLine(line, out start, out goal);
agents.Add(new Agent(agents.Count, grid[start], grid[goal]));
}
this.agents = agents.ToArray();
}
public Agent(int id, NodeL start, NodeL goal)
{
this.ID = id;
this.start = start;
this.goal = goal;
}
public virtual DIRECTION translateMove(NodeL from, NodeL to, ref View v)
{
return(0);
}
/// <summary>
/// Heuristic function - count Manhattan metrics between given nodes.
/// </summary>
/// <param name="n"></param>
/// <param name="goal"></param>
/// <returns></returns>
int heuristic(NodeL n, NodeL goal)
{
return(Math.Abs(n.x - goal.x) + Math.Abs(n.y - goal.y));
}
public Path LowLevelSearch(Agent a, List <Constraint> cs)
{
int solutionTime = int.MaxValue;
var paths = new List <List <NodeL> >();
int time = 0;
Queue <NodeL> open = new Queue <NodeL>();
HashSet <NodeL> closed = new HashSet <NodeL>(); // todo use this?
open.Enqueue(a.start);
while (open.Any())
{
time++;
var current = open.Dequeue();
closed.Add(current);
if (current.time > solutionTime)
{
continue;
}
if (current.id == a.goal.id)
{
if (current.time > solutionTime)
{
continue;
}
solutionTime = current.time;
// found path
var path = new List <NodeL>();
var temp = current;
while (temp != null)
{
path.Add(temp);
temp = temp.previous;
}
path.Reverse();
return(new Path(path));
}
else
{
foreach (var item in current.neighbours)
{
bool found = cs.Any(x => x.timeStep == (current.time + 1) && a.ID == x.a.ID && x.nodeId == item.id);
if (found)
{
continue;
}
if (item.id == a.goal.id)
{
if (current.time > solutionTime)
{
continue;
}
solutionTime = current.time;
// found path
var path = new List <NodeL>();
path.Add(item);
var temp = current;
while (temp != null)
{
path.Add(temp);
temp = temp.previous;
}
path.Reverse();
return(new Path(path));
}
else
{
if (current.previous != null && item.id == current.previous.id)
{
continue;// do not allow immediately going back
}
var newNode1 = new NodeL(item.id, item.x, item.y, current.time + 1, item.neighbours, current);
//newNode1.neighbours = new List<NodeL>(item.neighbours);
//newNode1.previous = current;.
//newNode1.time = current.time + 1;
if (!closed.Contains(newNode1))
{
open.Enqueue(newNode1);
}
}
}
if (!cs.Any(x => x.timeStep == (current.time + 1) && a.ID == x.a.ID && x.nodeId == current.id))
{
// cycle also fine
var newNode = new NodeL(current.id, current.x, current.y, current.time + 1, current.neighbours, current);
//newNode.previous = current;
//newNode.neighbours = new List<NodeL>(current.neighbours);
//newNode.time = current.time + 1;
open.Enqueue(newNode);
}
}
}
return(null);
}
/// <summary>
/// Searches path in grid for agent a.
/// </summary>
/// <param name="a"></param>
/// <returns></returns>
public Path LowLevelSearch(Agent a, List <Constraint> cs)
{
const int MAX = 100;
// closed nodes
HashSet <NodeL> closed = new HashSet <NodeL>();
// opened nodes
HashSet <NodeL> opened = new HashSet <NodeL>();
opened.Add(a.start);
// key node can be reached from value node the most efficiently (most efficient previous step)
//Dictionary<NodeL, NodeL> cameFrom = new Dictionary<NodeL, NodeL>();
// for each node, the cost of getting from the start node to that node
Dictionary <NodeL, int> gScore = new Dictionary <NodeL, int>();
gScore.Add(a.start, 0);
// for each node, total cost of getting from start to goal through that node
// consists of gScore and heuristic estimate
Dictionary <NodeL, int> fScore = new Dictionary <NodeL, int>();
fScore.Add(a.start, heuristic(a.start, a.goal));
// main loop
while (opened.Count > 0)
{
NodeL current = findMin(fScore, opened);
// goal test
if (current.id == a.goal.id)
{
var path = new List <NodeL>();
var temp = current;
while (temp != null)
{
path.Add(temp);
temp = temp.previous;
}
// path.Reverse();
return(new Path(path));
// return recontructPath(cameFrom, a.start,/* cs,*/ a.ID);
}
opened.Remove(current);
if (current.time > MAX)
{
continue;
}
// A* will never finish if there is no solution
// because of the time, we will never visit all nodes
// solution ?? limit time to max time of conflict? after that keep time constant => no duplicate nodes
closed.Add(current);
var nodes = current.neighbours.Select(x => new NodeL(x.id, x.x, x.y, current.time + 1, x.neighbours, current)).ToList();
var currentNode = new NodeL(current.id, current.x, current.y, current.time + 1, current.neighbours, current);
nodes.Add(currentNode);
// go through all neighbours of the current node
foreach (NodeL neighbour in nodes)
{
if (cs.Any(x => x.timeStep == (current.time + 1) && a.ID == x.a.ID && x.nodeId == neighbour.id))
{
continue;
}
// goal test
if (neighbour.id == a.goal.id)
{
var path = new List <NodeL>();
var temp = neighbour;
while (temp != null)
{
path.Add(temp);
temp = temp.previous;
}
path.Reverse();
return(new Path(path));
// return recontructPath(cameFrom, a.start,/* cs,*/ a.ID);
}
//neighbour.time++;
// control if it is not closed
if (closed.Contains(neighbour))
{
continue;
}
/*bool isOK = true;
*
* // control if there is not conflict with constraints
* foreach (Constraint c in cs)
* {
* if (c.a == a && c.timeStep == neighbour.time && c.nodeId == neighbour.id)
* {
* isOK = false;
* allConstraintsOK = false;
* break;
* }
* }
*
* if (!isOK)
* {
* continue;
* }
*/
//neighbour.previous = currentNode;
// add node to opened
if (!opened.Contains(neighbour))
{
opened.Add(neighbour);
}
// recomputation
int tentative_gScore = gScore[current] + 1;
if (gScore.ContainsKey(neighbour) && tentative_gScore >= gScore[neighbour])
{
continue;
}
else // found better past - record it
{
// refresh cameFrom
/*if (cameFrom.ContainsKey(neighbour))
* {
* cameFrom[neighbour] = current;
* }
* else
* {
* cameFrom.Add(neighbour, current);
* }*/
// refresh gScore
if (gScore.ContainsKey(neighbour))
{
gScore[neighbour] = tentative_gScore;
}
else
{
gScore.Add(neighbour, tentative_gScore);
}
// refresh fScore
if (fScore.ContainsKey(neighbour))
{
fScore[neighbour] = gScore[neighbour] + heuristic(neighbour, a.start);
}
else
{
fScore.Add(neighbour, gScore[neighbour] + heuristic(neighbour, a.start));
}
}
}
/*if (!allConstraintsOK)
* {
* closed.Remove(current);
* opened.Add(current);
* gScore[current]++;
* fScore[current]++;
* }*/
}
return(null);
}
