private static void Main(string[] args)
{
var size = 15;
var heuristic = new HeuristicAlgorithm();
heuristic.RunAlgorithm(size);
}
/// <summary>
/// Calculates the Heuristic Score based on several different Algorithms.
/// </summary>
/// <param name="algorithm">See <see cref="HeuristicAlgorithm"/> for more information.</param>
/// <param name="from">The Node we're coming from. Can be null if this is the start.</param>
/// <param name="w1">First waypoint (typically "start" or your Current)</param>
/// <param name="w2">Second waypoint (the "goal" or "target")</param>
/// <returns></returns>
// References:
// - https://brilliant.org/wiki/a-star-search/
// - http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html
// - https://www.growingwiththeweb.com/2012/06/a-pathfinding-algorithm.html
private float CalculateHeuristic(HeuristicAlgorithm algorithm, Node from, Waypoint w1, Waypoint w2, bool ignoreWaypointHeuristic = false)
{
float cost = 0, max = 0, min = 0, h1 = 0, h2 = 0;
switch (algorithm)
{
case HeuristicAlgorithm.Vector3Distance:
return(Vector3.Distance(w1.transform.position, w2.transform.position) + HScore(w1, ignoreWaypointHeuristic));
case HeuristicAlgorithm.Manhattan:
return((Mathf.Abs(w1.transform.position.x - w2.transform.position.x) +
Mathf.Abs(w1.transform.position.y - w2.transform.position.y) +
Mathf.Abs(w1.transform.position.z - w2.transform.position.z))
+ HScore(w1, ignoreWaypointHeuristic));
case HeuristicAlgorithm.CostOfMovement:
h1 = HScore(w1, ignoreWaypointHeuristic);
h2 = HScore(w2, ignoreWaypointHeuristic);
cost = Mathf.Max(Mathf.Abs(h2 - h1),
Mathf.Abs(h1 - h2));
return(Vector3.Distance(w1.transform.position, w2.transform.position) + cost);
case HeuristicAlgorithm.InverseCOM:
h1 = HScore(w1, ignoreWaypointHeuristic) * -1;
h2 = HScore(w2, ignoreWaypointHeuristic) * -1;
cost = Mathf.Min(h1 - h2, h2 - h1);
return(Vector3.Distance(w1.transform.position, w2.transform.position) + cost);
case HeuristicAlgorithm.DiagonalDistanceUniform:
max = Mathf.Max(
Mathf.Max(Mathf.Abs(w1.transform.position.x - w2.transform.position.x),
Mathf.Abs(w1.transform.position.y - w2.transform.position.y)),
Mathf.Abs(w1.transform.position.z - w2.transform.position.z));
return(HScore(w1, ignoreWaypointHeuristic) * max);
case HeuristicAlgorithm.DiagonalDistance:
max = Mathf.Max(
Mathf.Max(Mathf.Abs(w1.transform.position.x - w2.transform.position.x),
Mathf.Abs(w1.transform.position.y - w2.transform.position.y)),
Mathf.Abs(w1.transform.position.z - w2.transform.position.z));
min = Mathf.Min(
Mathf.Min(Mathf.Abs(w1.transform.position.x - w2.transform.position.x),
Mathf.Abs(w1.transform.position.y - w2.transform.position.y)),
Mathf.Abs(w1.transform.position.z - w2.transform.position.z));
return((HScore(w1, ignoreWaypointHeuristic) * 1.414f) * (max - min));
case HeuristicAlgorithm.HScore:
return(HScore(w1, ignoreWaypointHeuristic));
case HeuristicAlgorithm.Incremental:
return((from != null ? from.score : 0) + HScore(w1, ignoreWaypointHeuristic));
case HeuristicAlgorithm.Drunk:
h1 = HScore(w1, ignoreWaypointHeuristic);
h2 = HScore(w2, ignoreWaypointHeuristic);
return(Random.Range(Mathf.Min(h1, h2), Mathf.Max(h1, h2)));
}
return(-1); // what!?
}
/// <summary>
///
/// </summary>
/// <param name="start"></param>
/// <param name="end"></param>
/// <param name="pathCallback"></param>
/// <param name="heuristicAlgorithm">See <see cref="HeuristicAlgorithm"/> for more information.</param>
/// <param name="ignoreWaypointHeuristic"></param>
/// <returns></returns>
public IEnumerator FindPath(Waypoint start,
Waypoint end,
System.Action <List <Waypoint> > pathCallback,
HeuristicAlgorithm heuristicAlgorithm = HeuristicAlgorithm.Vector3Distance,
bool ignoreWaypointHeuristic = false)
{
// Return Value (in Node form)
List <Node> path = new List <Node>();
// Open Waypoints
List <Node> open = new List <Node>();
// Closed Waypoints
Queue <Waypoint> closed = new Queue <Waypoint>();
// Start with adding our start waypoint to the list of open waypoints
open.Add(new Node(start, null, CalculateHeuristic(heuristicAlgorithm, null, start, end, ignoreWaypointHeuristic)));
while (open.Count > 0)
{
// Step 1: Get the next waypoint to process, based on the heuristic
Node nextWaypoint = null;
// Our psuedo-Priority Queue Linq Query will order everything in the open list by score.
// Meaning the top is the best score. Meaning no loop required to search.
IEnumerable <Node> query = open.OrderBy(node => node.score);
nextWaypoint = query.First();
if (nextWaypoint.waypoint == end)
{
path.Add(nextWaypoint);
break;
}
open.Remove(nextWaypoint);
if (nextWaypoint == null)
{
yield return(null);
continue;
}
closed.Enqueue(nextWaypoint.waypoint);
// Step 2: Add the connections that haven't been visited to the open queue
foreach (Waypoint c in nextWaypoint.waypoint.connections)
{
if (closed.Contains(c) == false)
{
open.Add(new Node(c, nextWaypoint, CalculateHeuristic(heuristicAlgorithm, nextWaypoint, c, end, ignoreWaypointHeuristic)));
}
}
yield return(null);
}
pathCallback(BuildFinalPath(path));
}
public int Heuristic(Node node, HeuristicAlgorithm heuristic)
{
if (heuristic == HeuristicAlgorithm.Misplaced)
{
if (node.puzzle.Length == 9)
{
int[] goal = { 1, 2, 3, 4, 5, 6, 7, 8, 0 };
int h1 = GetNodeDepth(node);
int h2 = 0;
int expected = 0;
for (int i = 0; i < node.puzzle.Length; i++)
{
expected++;
if (node.puzzle[i] != 0 && node.puzzle[i] != expected)
{
h2++;
}
}
return(h1 + h2);
}
if (node.puzzle.Length == 16)
{
int[] goal = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 0 };
int h1 = GetNodeDepth(node);
int h2 = 0;
int expected = 0;
for (int i = 0; i < node.puzzle.Length; i++)
{
expected++;
if (node.puzzle[i] != 0 && node.puzzle[i] != expected)
{
h2++;
}
}
return(h1 + h2);
}
}
if (heuristic == HeuristicAlgorithm.Euclid)
{
if (node.puzzle.Length == 9)
{
int[] goal = { 1, 2, 3, 4, 5, 6, 7, 8, 0 };
int h1 = GetNodeDepth(node);
int h2 = 0;
int expected = 0;
for (int i = 0; i < node.puzzle.Length; i++)
{
int val_curr = node.puzzle[i];
int val_goal = Array.IndexOf(goal, val_curr);
expected++;
if (val_curr != expected)
{
int row_curr = i / 3;
int col_curr = i % 3;
int row_goal = val_goal / 3;
int col_goal = val_goal % 3;
h2 += (row_curr - row_goal) * (row_curr - row_goal) + (col_curr - col_goal) * (col_curr - col_goal);
}
}
//return h1 + Convert.ToInt16(Math.Sqrt(Math.Pow(h2, 2.0)));
//return h1 + Convert.ToInt16(Math.Sqrt((h2)));
//return h1 + (h2 * h2);
return(h1 + h2);
}
if (node.puzzle.Length == 16)
{
int[] goal = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 0 };
int h1 = GetNodeDepth(node);
int h2 = 0;
int expected = 0;
for (int i = 0; i < node.puzzle.Length; i++)
{
int val_curr = node.puzzle[i];
int val_goal = Array.IndexOf(goal, val_curr);
expected++;
if (val_curr != 0 && val_goal != expected)
{
int row_curr = i / 4;
int col_curr = i % 4;
int row_goal = val_goal / 4;
int col_goal = val_goal % 4;
h2 += Math.Abs(row_curr - row_goal) + Math.Abs(col_curr - col_goal);
}
}
//return h1 + Convert.ToInt16(Math.Sqrt(Math.Pow(h2, 2.0)));
//return h1 + Convert.ToInt16(Math.Sqrt((h2 * h2)));
return(h1 + (h2 * h2));
}
}
if (heuristic == HeuristicAlgorithm.Manhattan)
{
if (node.puzzle.Length == 9)
{
int[] goal = { 1, 2, 3, 4, 5, 6, 7, 8, 0 };
int h1 = GetNodeDepth(node);
int h2 = 0;
int expected = 0;
for (int i = 0; i < node.puzzle.Length; i++)
{
int val_curr = node.puzzle[i];
int val_goal = Array.IndexOf(goal, val_curr);
expected++;
if (val_curr != 0 && val_curr != expected)
{
int row_curr = i / 3;
int col_curr = i % 3;
int row_goal = val_goal / 3;
int col_goal = val_goal % 3;
h2 += Math.Abs(row_curr - row_goal) + Math.Abs(col_curr - col_goal);
}
}
return(h1 + h2);
}
if (node.puzzle.Length == 16)
{
int[] goal = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 0 };
int h1 = GetNodeDepth(node);
int h2 = 0;
int expected = 0;
for (int i = 0; i < node.puzzle.Length; i++)
{
int val_curr = node.puzzle[i];
int val_goal = Array.IndexOf(goal, val_curr);
expected++;
if (val_curr != 0 && val_goal != expected)
{
int row_curr = i / 4;
int col_curr = i % 4;
int row_goal = val_goal / 4;
int col_goal = val_goal % 4;
h2 += Math.Abs(row_curr - row_goal) + Math.Abs(col_curr - col_goal);
}
}
return(h1 + h2);
}
}
if (heuristic == HeuristicAlgorithm.Chebyshev)
{
if (node.puzzle.Length == 9)
{
int[] goal = { 1, 2, 3, 4, 5, 6, 7, 8, 0 };
int h1 = GetNodeDepth(node);
int h2 = 0;
int expected = 0;
for (int i = 0; i < node.puzzle.Length; i++)
{
int val_curr = node.puzzle[i];
int val_goal = Array.IndexOf(goal, val_curr);
expected++;
if (val_curr != 0 && val_goal != expected)
{
int row_curr = i / 3;
int col_curr = i % 3;
int row_goal = val_goal / 3;
int col_goal = val_goal % 3;
//int temp = Math.Abs(Math.Abs(row_curr - row_goal) - Math.Abs(col_curr - col_goal));
h2 += Math.Max(Math.Abs(row_curr - row_goal), Math.Abs(col_curr - col_goal));
}
}
return(h1 + h2);
}
if (node.puzzle.Length == 16)
{
int[] goal = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 0 };
int h1 = GetNodeDepth(node);
int h2 = 0;
int expected = 0;
for (int i = 0; i < node.puzzle.Length; i++)
{
int val_curr = node.puzzle[i];
int val_goal = Array.IndexOf(goal, val_curr);
expected++;
if (val_curr != 0 && val_goal != expected)
{
int row_curr = i / 4;
int col_curr = i % 4;
int row_goal = val_goal / 4;
int col_goal = val_goal % 4;
//int temp = Math.Abs(Math.Abs(row_curr - row_goal) - Math.Abs(col_curr - col_goal));
int temp = Math.Max(Math.Abs(row_curr - row_goal), Math.Abs(col_curr - col_goal));
if (h2 < temp)
{
h2 = temp;
}
}
}
return(h1 + h2);
}
}
return(0);
}
// A* step-to-auto
public List <Node> AStarSearch(Node root, TextBox sTb, TextBox wTb, int step, bool bystep, int maxdepth, int heuristic_index)
{
if (first)
{
first = false;
heuristic = SetHeuristic(heuristic_index);
PathToSolution = new List <Node>();
//ClosedList = new List<Node>();
ClosedList = new HashSet <Node>();
priorityQueue = new FastPriorityQueue <Node>(500000);
priorityQueue.Enqueue(root, Heuristic(root, heuristic));
}
while (priorityQueue.Count > 0 && step_in <= step)
{
var vertex = priorityQueue.Dequeue();
ClosedList.Add(vertex);
#region info
if (bystep)
{
vertex.PrintPuzzle(wTb);
}
vertex.ExpandMoveStack();
info_node[0] += vertex.children.Count;
info_node[3] = GetNodeDepth(vertex);
if (!depthCount.ContainsKey(info_node[3]))
{
depthCount.Add(info_node[3], info_node[0]);
}
else if (info_node[3] != 1 && info_node[3] != 2)
{
depthCount[info_node[3]] = info_node[0];
}
#endregion
if (vertex.IsFinish())
{
PathTrace(PathToSolution, vertex);
sTb.Invoke((MethodInvoker) delegate
{
sTb.AppendText(Environment.NewLine + "Solution found" + Environment.NewLine);
});
return(PathToSolution);
}
foreach (var child in vertex.children)
{
if (!Contains(priorityQueue, child) && !Contains(ClosedList, child) && GetNodeDepth(child) <= maxdepth)
{
info_node[1]++;
priorityQueue.Enqueue(child, Heuristic(child, heuristic));
}
}
if (priorityQueue.Count == 0)
{
sTb.Invoke((MethodInvoker) delegate
{
sTb.AppendText(Environment.NewLine + "No solution found" + Environment.NewLine);
});
}
step_in++;
}
return(PathToSolution);
}
// A* full auto
public List <Node> AStarSearch(Node root, TextBox sTb, TextBox wTb, int maxdepth, int heuristic_index, CancellationToken cancellationToken)
{
heuristic = SetHeuristic(heuristic_index);
PathToSolution = new List <Node>();
ClosedList = new HashSet <Node>();
//List<Node> ClosedList = new List<Node>();
priorityQueue = new FastPriorityQueue <Node>(100000000);
//SimplePriorityQueue<Node> priorityQueue = new SimplePriorityQueue<Node>();
priorityQueue.Enqueue(root, Heuristic(root, heuristic));
//info_node[3] = 1;
//depthCount.Add(info_node[3], info_node[0]);
while (priorityQueue.Count > 0)
{
if (cancellationToken.IsCancellationRequested)
{
sTb.Invoke((MethodInvoker) delegate
{
sTb.Text = "Cancelled";
});
return(PathToSolution);
}
var vertex = priorityQueue.Dequeue();
current = vertex;
ClosedList.Add(vertex);
if (vertex.IsFinish())
{
PathTrace(PathToSolution, vertex);
sTb.Invoke((MethodInvoker) delegate
{
sTb.AppendText(Environment.NewLine + "Solution found" + Environment.NewLine);
});
return(PathToSolution);
}
vertex.ExpandMoveStack();
#region info
info_node[0] += vertex.children.Count;
info_node[3] = GetNodeDepth(vertex);
if (!depthCount.ContainsKey(info_node[3]))
{
depthCount.Add(info_node[3], info_node[0]);
}
else if (info_node[3] != 1 && info_node[3] != 2)
{
depthCount[info_node[3]] = info_node[0];
}
#endregion
foreach (var child in vertex.children)
{
if (!ClosedList.Contains(child) && GetNodeDepth(child) <= maxdepth /*&& !priorityQueue.Contains(child)*/)
{
info_node[1] = ClosedList.Count;
priorityQueue.Enqueue(child, Heuristic(child, heuristic));
}
else
{
info_node[0]--;
}
/*
* if (!Contains(priorityQueue, child) && !Contains(ClosedList, child) && )
* {
* info_node[1]++;
* priorityQueue.Enqueue(child, Heuristic(child, heuristic));
* }
*/
}
while (priorityQueue.Count > 0 && ClosedList.Contains(priorityQueue.First))
{
priorityQueue.Dequeue();
}
}
return(PathToSolution);
}
