diff --git a/Pathfinding Visualizer/Assets/Scripts/Main.cs b/Pathfinding Visualizer/Assets/Scripts/Main.cs index 42866f0..bcf897a 100644 --- a/Pathfinding Visualizer/Assets/Scripts/Main.cs +++ b/Pathfinding Visualizer/Assets/Scripts/Main.cs @@ -146,8 +146,7 @@ public class Main : MonoBehaviour if ( true ) { - Pathfinder astar = new Pathfinder(world, agent, obstacles, samples, 0, 0); - astar.go(); + Pathfinder path = new Pathfinder(world, agent, obstacles, samples, 0, 0); } } diff --git a/Pathfinding Visualizer/Assets/Scripts/PathNode.cs b/Pathfinding Visualizer/Assets/Scripts/PathNode.cs index 4ad2070..b1b1aa1 100644 --- a/Pathfinding Visualizer/Assets/Scripts/PathNode.cs +++ b/Pathfinding Visualizer/Assets/Scripts/PathNode.cs @@ -4,19 +4,19 @@ using System.Collections.Generic; using System.Linq; using UnityEngine; - class Node : IComparable + public class Node //: IComparable { // helper variables just to make indexing arrays easier to read public static int row = 0; public static int col = 1; - Node parent; - Vector2Int agent; - List samples; - char lastMove; // for output/animation - int distanceTraveled; // for calculating f(n) value - double heuristic; - public double fn; // distance traveled + heuristic value + public Node parent; + public Vector2Int agent; + public List samples; + public char lastMove; // for output/animation + public int distanceTraveled; // for calculating f(n) value + public double heuristic; + public static double fn; // distance traveled + heuristic value bool canSample; // for expansion // construct node :) @@ -28,7 +28,7 @@ using UnityEngine; this.lastMove = lastMove; this.distanceTraveled = distanceTraveled; this.heuristic = heuristic; - this.fn = distanceTraveled + heuristic; + fn = distanceTraveled + heuristic; } // determine valid moves and collect children to be sent back to search @@ -101,7 +101,6 @@ using UnityEngine; { Node child = new Node(this, this.agent, this.samples, 'S', this.distanceTraveled + 1, this.heuristic); - // PROBLEM? child.samples.RemoveAt(onSample.y); //SampleWorld.nodesGenerated++; @@ -191,18 +190,18 @@ using UnityEngine; } // we use to override comparisons for priorityQueues so that our heuristic calculations are actually used - public int CompareTo(Node other) +/* public int CompareTo(Node other) { if (this.fn > other.fn) - return 1; + return 1; else - return -1; - } + return -1; + }*/ } // helper class for Node - class NodeComparator : Comparer +/* class NodeComparator : Comparer { // used to sort nodes based on sum of distance traveled + heuristic estimation of work left public override int Compare(Node a, Node b) @@ -212,7 +211,7 @@ using UnityEngine; else return -1; } - } + }*/ // helper class for Node public class State { diff --git a/Pathfinding Visualizer/Assets/Scripts/Pathfinder.cs b/Pathfinding Visualizer/Assets/Scripts/Pathfinder.cs index eae7233..884a05a 100644 --- a/Pathfinding Visualizer/Assets/Scripts/Pathfinder.cs +++ b/Pathfinding Visualizer/Assets/Scripts/Pathfinder.cs @@ -1,6 +1,8 @@ using System.Collections; using System.Collections.Generic; using UnityEngine; +using System.Linq; + public class Pathfinder : MonoBehaviour { @@ -32,14 +34,17 @@ public class Pathfinder : MonoBehaviour algorithm = algo; heuristic = heu; + + printInfo(); + + StartWork(); + } - - public void go() + public void printInfo() { - Debug.Log("Agent: " + agent[0] + "," + agent[1]); - for(int i = 0; i < obstacles.Count; i++) + for (int i = 0; i < obstacles.Count; i++) { Debug.Log("Obstacle " + i + ": " + obstacles[i].x + ", " + obstacles[i].y); } @@ -48,6 +53,121 @@ public class Pathfinder : MonoBehaviour { Debug.Log("Sample " + i + ": " + samples[i].x + ", " + samples[i].y); } + } + + public void StartWork() + { + Node initialState = new Node(null, agent, samples, '0', 0, 0); + Stack result = new Stack(); + + if (algorithm == 0) + { + result = StartAStar(initialState); + + } + } + + public static Stack StartAStar(Node initialState) + { + Stack solution = new Stack(); + + Queue open = new Queue(); + + + + //PriorityQueue open = new PriorityQueue(); + + open.Enqueue(initialState); + int cap = 10000; + while (true && cap > 0) + { + if (!open.Any()) + return solution; + + Node currentNode = open.Dequeue(); + + // check whether we are in goal state + if (currentNode.samples.Count() == 0) + { + + var orderedList = open.OrderBy(Node => Node.fn); + + while (currentNode.parent != null) + { + solution.Push(currentNode); + currentNode = currentNode.parent; + } + return solution; + } + + // if not, expand children and continue + List children = currentNode.expand(-1); + + if (heuristic.Equals("h0")) + h0(children); + /* else if (heuristic.Equals("h1")) + h1(children); + else if (heuristic.Equals("h2")) + h2(children);*/ + + foreach (Node child in children) + { + open.Enqueue(child); + } + cap--; + } + } + // HEURISTICS FOR ASTAR + public static List h0(List children) + { + List sorted = new List(); + + for (int i = 0; i < children.Count(); i++) + { + // estimated moves left: zero + children[i].heuristic = 0; + sorted.Add(children[i]); + } + + sorted = sorted.OrderBy(Node => Node.fn).ToList(); + + return sorted; + } + +/* public static List h1(List children) + { + Queue sorted = new PriorityQueue<>(new NodeComparator()); + + for (int i = 0; i < children.size(); i++) + { + // estimated moves left: samples left + children.get(i).heuristic = children.get(i).samples.size(); + sorted.add(children.get(i)); + } + return children; + } + + public static Queue h2(List children) + { + Queue sorted = new PriorityQueue<>(new NodeComparator()); + + if (children.size() == 0) + return sorted; + + Node child; + + for (int i = 0; i < children.size(); i++) + { + // estimated moves left: + // distance to nearest sample + 1 (+1 to account for sample action) + child = children.get(i); + child.heuristic = Node.distance(child.agent, child.nearestSample()) + 1; + + sorted.add(child); + } + + return sorted; + }*/ }