public void AddPath(Path path)
{
try
{
Paths.Add(path.Identifier, path);
}
catch (ArgumentException)
{
Paths.Remove(path.Identifier);
Paths.Add(path.Identifier, path);
}
}
public bool RemovePath(Path path)
{
bool result = Paths.Remove(path.Identifier);
return result;
}
public void ShortestPathToBlockedEdge()
{
Graph graph = new Graph(1);
Node a = new Node("a");
Node b = new Node("b");
Node c = new Node("c");
Node d = new Node("d");
Node e = new Node("e");
graph.AddNode(a);
graph.AddNode(b);
graph.AddNode(c);
graph.AddNode(d);
graph.AddNode(e);
graph.AddUndirectedEdge(new Tuple<Node, int>(a, 1), new Tuple<Node, int>(b, 3), 1);
graph.AddUndirectedEdge(new Tuple<Node, int>(b, 2), new Tuple<Node, int>(c, 0), 2);
graph.AddUndirectedEdge(new Tuple<Node, int>(c, 3), new Tuple<Node, int>(d, 1), 3); //block this
graph.AddUndirectedEdge(new Tuple<Node, int>(d, 0), new Tuple<Node, int>(e, 2), 200);
graph.AddUndirectedEdge(new Tuple<Node, int>(e, 1), new Tuple<Node, int>(a, 3), 100);
graph.BlockEdge(c, d);
Path expectedPath = new Path()
{
Identifier = 1,
Nodes = new List<Node>(){a, b, c}
};
Path actualPath = graph.PathToBlockedEdge(a, graph.BlockedEdges.FirstOrDefault());
actualPath.Identifier = 1;
Assert.IsTrue(expectedPath.Equals(actualPath));
}
public void ShortestPathThreeNodesTwoPaths()
{
Graph g = new Graph(1);
Node a = new Node("a");
Node b = new Node("b");
Node c = new Node("c");
Edge ab = new Edge(20);
Edge ac = new Edge(10);
Edge cb = new Edge(5);
g.AddNode(a);
g.AddNode(b);
g.AddNode(c);
g.AddUndirectedEdge(new Tuple<Node, int>(a, 0), new Tuple<Node, int>(b, 0), ab);
g.AddUndirectedEdge(new Tuple<Node, int>(a, 1), new Tuple<Node, int>(c, 0), ac);
g.AddUndirectedEdge(new Tuple<Node, int>(c, 1), new Tuple<Node, int>(b, 1), cb);
Path actual = g.ShortestPath(a, b);
Path expected = new Path();
expected.Nodes.Add(a);
expected.Nodes.Add(c);
expected.Nodes.Add(b);
Assert.IsTrue(expected.Equals(actual));
}
public void ShortestPathTwoNodes()
{
Graph g = new Graph(1);
Node a = new Node("a");
Node b = new Node("b");
Edge ab = new Edge(10);
g.AddNode(a);
g.AddNode(b);
g.AddUndirectedEdge(new Tuple<Node, int>(a, 0), new Tuple<Node, int>(b, 0), ab);
Path actual = g.ShortestPath(a, b);
Path expected = new Path();
expected.Nodes.Add(a);
expected.Nodes.Add(b);
Assert.IsTrue(actual.Equals(expected));
}
public Path ShortestPath(Node from, Node to, Node ignore = null)
{
if (from == null)
{
throw new NodeException("From-node cannot be null");
}
if (to == null)
{
throw new NodeException("To-node cannot be null");
}
// Make sure that the nodes are in the graph
if (!(Nodes.FindAll(x => x.Equals(from)).Count > 0 && Nodes.FindAll(x => x.Equals(to)).Count > 0))
{
throw new Exception("Either 'from'- or 'to'-node not in graph!");
}
List<Node> Q = new List<Node>();
List<KeyValuePair<Node, double>> distance = new List<KeyValuePair<Node, double>>();
List<KeyValuePair<Node, Node>> previous = new List<KeyValuePair<Node, Node>>();
foreach (Node node in Nodes)
{
if (node == null)
{
continue;
}
if (!node.Equals(from))
{
// Set the distance of all nodes to infinity
distance.Add(new KeyValuePair<Node, double>(node, Double.MaxValue));
// Set the previous node for all nodes to null (undefined)
previous.Add(new KeyValuePair<Node, Node>(node, null));
}
// Add the node to Q
Q.Add(node);
}
// The distance to the node "from" is always 0
distance.Add(new KeyValuePair<Node, double>(from, 0));
// While there are nodes to inspect
while (Q.Count > 0)
{
// Find the node with lowest distance in Q
Node selectedNode = Q.First();
double oldDistance = Int32.MaxValue;
foreach (Node node in Q)
{
if (node == null)
{
continue;
}
double newDistance = distance.Find(x => x.Key.Equals(node)).Value;
if (newDistance < oldDistance)
{
selectedNode = node;
oldDistance = newDistance;
}
}
// Remove the node from u
Q.Remove(selectedNode);
foreach (KeyValuePair<Node, Edge> neighbour in selectedNode.Neighbours)
{
if (neighbour.Key == null)
{
continue;
}
// If selected the from node, ignore the ignore node
if (ignore != null && selectedNode.Equals(from) && neighbour.Key.Equals(ignore))
{
continue;
}
// Find the distance to the selected
double distanceToSelectedNode = distance.Find(x => x.Key.Equals(selectedNode)).Value;
// Find the distance to the selected neighbour
double distanceToNeighbour = selectedNode.Neighbours.Find(x => x.Key != null && x.Key.Equals(neighbour.Key)).Value.Weight;
// Add the two distances
double alt = distanceToSelectedNode + distanceToNeighbour;
double neighbourDistance = distance.Find(x => x.Key.Equals(neighbour.Key)).Value;
// Check if this new distance (alt) is shorter than what we know
if(alt < neighbourDistance)
{
// Update the distance
distance.RemoveAll(x => x.Key.Equals(neighbour.Key));
distance.Add(new KeyValuePair<Node, double>(neighbour.Key, alt));
// Set previous of neighbour to selectednode
previous.RemoveAll(x => x.Key.Equals(neighbour.Key));
previous.Add(new KeyValuePair<Node, Node>(neighbour.Key, selectedNode));
}
}
}
// Create a new path
Path p = new Path();
// Start the path from the target (Path will later be inverted)
Node lastNode = to;
bool previousNode = true;
// Add the last node to path
p.Nodes.Add(lastNode);
// Continue the loop as long as there are a previous node
while (previousNode)
{
// Find the previous node
KeyValuePair<Node, Node> newPreviousNode = previous.Find(x => x.Key != null && x.Key.Equals(lastNode));
// Check if the last node is the start
// There is a path, everything went well
if (lastNode.Equals(from))
{
previousNode = false;
}
// The last node has a previous node, everything is good
// However, the path is still not finished, so continue the loop
else if (newPreviousNode.Value != null)
{
p.Nodes.Add(newPreviousNode.Value);
lastNode = newPreviousNode.Value;
}
// We are not at the start node, and not at a valid node,
// hence, there must be a gab in the path. Error!
else
{
throw new PathException("Cannot generate path. Some edges might be blocked.");
}
}
// Reserve the graph
p.Nodes.Reverse();
return p;
}
public ForkliftPath(Path path, Node initialNode = null)
{
if (path == null)
{
throw new Exception("Cannot convert null-path to ForkliftPath");
}
this.initialNode = initialNode;
// If the initial node is null, check for the node in the forklift property
if (this.initialNode == null)
{
this.initialNode = Database.Instance.Data.Forklifts.FirstOrDefault().Value.RearNode;
}
// Something is wrong, if the initial node is still null
if (this.initialNode == null)
{
throw new NodeException("Initial node is null");
}
// Check if the initial node is also the first node in the path
if (this.initialNode.Equals(path.Nodes.FirstOrDefault()))
{
throw new NodeException("First- and ignore node cannot be the same");
}
// Check if the initial node is a neighbour of the first node in the path
Node node = path.Nodes.FirstOrDefault();
if (node != null
&& (node.Neighbours.FindAll(x => x.Key != null && x.Key.Equals(this.initialNode)).Count == 0
&& node.BlockedNeighbours.FindAll(x => x.Key != null && x.Key.Equals(this.initialNode)).Count == 0))
{
throw new NodeException("Could not find initial node '" + this.initialNode.Name + "' in the neighbours of node '" + node.Name + "'");
}
this.path = path;
if (path.Nodes.Count <= 1)
{
return;
}
int initialIndex = 0;
// Check if the initial node is in neighbours
if (path.Nodes[0].Neighbours.FindAll(x => x.Key != null && x.Key.Equals(initialNode)).Count > 0)
{
initialIndex = path.Nodes[0].Neighbours.FindIndex(x => x.Key != null && x.Key.Equals(initialNode));
}
// The initial node was not in neighbours, then it must be in blocked neighbours
else
{
initialIndex = path.Nodes[0].BlockedNeighbours.FindIndex(x => x.Key != null && x.Key.Equals(initialNode));
}
int fromIndex = initialIndex;
int toIndex = -1;
for (int i = 0; i < path.Nodes.Count - 1; i++)
{
Node fromNode = null;
Node thisNode = path.Nodes[i];
Node toNode = path.Nodes[i+1];
if (i != 0)
{
fromNode = path.Nodes[i - 1];
fromIndex = thisNode.Neighbours.FindIndex(x => x.Key != null && x.Key.Equals(fromNode));
}
toIndex = thisNode.Neighbours.FindIndex(x => x.Key != null && x.Key.Equals(toNode));
forkliftPath.Add(navigationMatrix[fromIndex, toIndex]);
}
}
