public static void NeoPathfinding(Point P, Point Goal)
{
Util.PriorityQueue <Point> frontier = new Util.PriorityQueue <Point>();
frontier.Enqueue(P, 0);
Dictionary <Point, Point> came_from = new Dictionary <Point, Point>();
Dictionary <Point, int> cost_so_far = new Dictionary <Point, int>();
Dictionary <Point, Tool> toolset = new Dictionary <Point, Tool>();
//came_from.Add(new Point(P.X, P.Y), );
cost_so_far.Add(new Point(P.X, P.Y), 0);
toolset.Add(new Point(P.X, P.Y), Tool.Torch);
while (frontier.Count > 0)
{
Point current = frontier.Dequeue();
if (current == Goal)
{
if (toolset[current] != Tool.Torch)
{
Console.WriteLine("PART 2 T: " + (cost_so_far[current] + 11));
}
else
{
Console.WriteLine("PART 2: " + (cost_so_far[current]));
}
break;
}
List <Point> chkPoints = new List <Point>();
if (doesExist(onMyTop(current)))
{
chkPoints.Add(onMyTop(current));
}
if (doesExist(onMyLeft(current)))
{
chkPoints.Add(onMyLeft(current));
}
if (doesExist(onMyRight(current)))
{
chkPoints.Add(onMyRight(current));
}
if (doesExist(onMyBottom(current)))
{
chkPoints.Add(onMyBottom(current));
}
foreach (var next in chkPoints)
{
int new_cost = cost_so_far[current] + costCalc(toolset[current], next);
if (cost_so_far.ContainsKey(next) == false || new_cost < cost_so_far[next])
{
cost_so_far[next] = new_cost;
int priority = new_cost + Util.ManhattanDist(Goal, next);
frontier.Enqueue(next, priority);
came_from[next] = current;
if (isObstructed(next, toolset[current]))
{
toolset[next] = switchTool(toolset[current], next);
}
else
{
toolset[next] = toolset[current];
}
}
}
}
}
