public void Initialize(NavGraph graph)
{
int noOfNodes = graph.Nodes.Count;
nodeRoute = new List <int>();
nodeRoute.Capacity = noOfNodes;
nodeCosts = new List <float>();
nodeCosts.Capacity = noOfNodes;
traversedEdges = new List <GraphEdge>();
edgeQueue = new CustomPriorityQueue <GraphEdge>();
Path = new List <int>();
//adds elements to the nodeCosts list, starting at the maximum possible amount so that untraversed nodes will always start off with larger values than traversed ones
for (int i = 0; i < nodeCosts.Capacity; i++)
{
float tempadd = float.MaxValue;
nodeCosts.Add(tempadd);
//nodeCosts[i] = float.MaxValue;
}
//adding default nodes to the route list
for (int i = 0; i < nodeRoute.Capacity; i++)
{
int tempadd = 0;
nodeRoute.Add(tempadd);
}
for (int i = 0; i < traversedEdges.Count; i++)
{
traversedEdges[i] = null;
}
}
public IEnumerable <T> FindPath <T>(ISearchProblem <T> problem)
{
var frontier = new CustomPriorityQueue <SearchNode <T> >();
var predecessors = new Dictionary <T, T>();
var costs = new Dictionary <T, float>();
var initialNode = new SearchNode <T>(problem.InitialState, 0);
frontier.Enqueue(initialNode);
predecessors[problem.InitialState] = default;
costs[problem.InitialState] = 0;
while (frontier.Any())
{
var node = frontier.Dequeue();
if (problem.IsGoalState(node.State))
{
return(Path.Build(problem.InitialState, node.State, predecessors));
}
var successors = problem.GetSuccessors(node.State);
foreach (var successor in successors)
{
var newCost = costs[node.State] + successor.Cost;
if (costs.TryGetValue(successor.State, out var cost) && cost < newCost)
{
continue;
}
costs[successor.State] = newCost;
predecessors[successor.State] = node.State;
var heuristic = problem.CalculateHeuristic(successor.State);
var priority = newCost + heuristic;
var successorNode = new SearchNode <T>(successor.State, priority);
frontier.Enqueue(successorNode);
}
}
return(null);
}
public IEnumerable <T> FindPath <T>(ISearchProblem <T> problem)
{
var frontier = new CustomPriorityQueue <SearchNode <T> >();
var cameFrom = new Dictionary <T, T>();
var costSoFar = new Dictionary <T, float>();
var firstNode = new SearchNode <T>(problem.InitialState, 0);
frontier.Enqueue(firstNode);
cameFrom[problem.InitialState] = default;
costSoFar[problem.InitialState] = 0;
while (frontier.Any())
{
var node = frontier.Dequeue();
if (problem.IsGoalState(node.State))
{
return(Path.Build(problem.InitialState, node.State, cameFrom));
}
var successors = problem.GetSuccessors(node.State);
foreach (var successor in successors)
{
var newCost = costSoFar[node.State] + successor.Cost;
if (costSoFar.TryGetValue(successor.State, out var cost) && newCost >= cost)
{
continue;
}
var action = new SearchNode <T>(successor.State, newCost);
frontier.Enqueue(action);
costSoFar[successor.State] = newCost;
cameFrom[successor.State] = node.State;
}
}
return(null);
}
public IEnumerable <T> FindPath <T>(ISearchProblem <T> problem)
{
var frontier = new CustomPriorityQueue <SearchNode <T> >();
var cameFrom = new Dictionary <T, T>();
var initialNode = new SearchNode <T>(problem.InitialState, 0);
frontier.Enqueue(initialNode);
cameFrom[problem.InitialState] = default;
while (frontier.Any())
{
var node = frontier.Dequeue();
if (problem.IsGoalState(node.State))
{
return(Path.Build(problem.InitialState, node.State, cameFrom));
}
var successors = problem.GetSuccessors(node.State);
foreach (var successor in successors)
{
if (cameFrom.ContainsKey(successor.State))
{
continue;
}
var priority = problem.CalculateHeuristic(successor.State);
var successorNode = new SearchNode <T>(successor.State, priority);
frontier.Enqueue(successorNode);
cameFrom[successor.State] = node.State;
}
}
return(null);
}
static void Main(string[] args)
{
// create some test data for the stack and the queue
string[] text = new string[3];
text[0] = "De eerste string";
text[1] = "De tweede string";
text[2] = "De derde string";
#region CustomStack test
Console.WriteLine("CustomStack test (LIFO)");
// create a stack of the type string
CustomStack <string> stack = new CustomStack <string>();
// add the strings to the stack
for (int i = 0; i <= text.GetUpperBound(0); i++)
{
stack.Push(text[i]);
}
// remove and show the order of removed items
for (int i = 0; i <= text.GetUpperBound(0); i++)
{
Console.WriteLine("- " + stack.Pop());
}
#endregion
Console.WriteLine();
#region CustomQueue test
Console.WriteLine("CustomQueue test (FIFO)");
// create a generic questom queue
CustomQueue <string> queue = new CustomQueue <string>();
// add items to the queue
for (int i = 0; i <= text.GetUpperBound(0); i++)
{
queue.EnQueue(text[i]);
}
// remove the items and show the position
for (int i = 0; i <= text.GetUpperBound(0); i++)
{
Console.WriteLine("- " + queue.Peek());
queue.DeQueue();
}
#endregion
Console.WriteLine();
#region CustomPriorityQueue test
Console.WriteLine("CustomPriorityQueue");
// create a CustomPriorityQueue to simulate a waiting room for patients
// where priority 0 is first
CustomPriorityQueue waitingRoom = new CustomPriorityQueue();
pqItem[] patients = new pqItem[4];
patients[0].name = "Klaas";
patients[0].priority = 1;
patients[1].name = "Gerald";
patients[1].priority = 3;
patients[2].name = "Tessa";
patients[2].priority = 0;
patients[3].name = "Yvonne";
patients[3].priority = 7;
// add all patients to the priority queue
for (int i = 0; i <= patients.GetUpperBound(0); i++)
{
waitingRoom.Enqueue(patients[i]);
}
// remove patients from waiting list in order of their priority
for (int i = 0; i <= patients.GetUpperBound(0); i++)
{
pqItem patient = (pqItem)waitingRoom.Dequeue();
Console.WriteLine(patient.priority + ": " + patient.name);
}
#endregion
Console.ReadKey();
}
