From ea904845a7a73c1b8574b41b42897ee521849421 Mon Sep 17 00:00:00 2001 From: Simon O'Shea Date: Wed, 9 Aug 2023 08:56:20 -0400 Subject: [PATCH] ASTAR WORKS WITH H0 AAAAAAAAAAAAAAAAAAAAAJHH --- Pathfinding Visualizer/Assets/Scripts/Main.cs | 4 +- .../Assets/Scripts/PathNode.cs | 19 +- .../Assets/Scripts/Pathfinder.cs | 277 +++++++++++++++++- 3 files changed, 285 insertions(+), 15 deletions(-) diff --git a/Pathfinding Visualizer/Assets/Scripts/Main.cs b/Pathfinding Visualizer/Assets/Scripts/Main.cs index 0733f2b..8cbb5ce 100644 --- a/Pathfinding Visualizer/Assets/Scripts/Main.cs +++ b/Pathfinding Visualizer/Assets/Scripts/Main.cs @@ -22,8 +22,8 @@ public class Main : MonoBehaviour void Start() { // Create world - int width = 5; - int height = 5; + int width = 4; + int height = 4; float cellSize = 10f; Vector3 origin = new Vector3(-10, -10); diff --git a/Pathfinding Visualizer/Assets/Scripts/PathNode.cs b/Pathfinding Visualizer/Assets/Scripts/PathNode.cs index c69e821..857886e 100644 --- a/Pathfinding Visualizer/Assets/Scripts/PathNode.cs +++ b/Pathfinding Visualizer/Assets/Scripts/PathNode.cs @@ -1,10 +1,10 @@ -using System; +/*using System; using System.Collections; using System.Collections.Generic; using System.Linq; using UnityEngine; - public class Node //: IComparable + public class Node { // helper variables just to make indexing arrays easier to read public static int row = 0; @@ -13,6 +13,7 @@ using UnityEngine; 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; @@ -194,18 +195,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; else 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) @@ -215,7 +216,7 @@ using UnityEngine; else return -1; } - }*/ + }*//* // helper class for Node public class State { @@ -228,10 +229,10 @@ using UnityEngine; this.samples = samples; // convert List into 2D array of ints [][] - /*for (int i = 0; i < samples.Count; i++) + *//*for (int i = 0; i < samples.Count; i++) { this.samples[i] = samples[i]; - }*/ + }*//* } public bool inClosed() @@ -264,4 +265,4 @@ using UnityEngine; return true; } - } + }*/ diff --git a/Pathfinding Visualizer/Assets/Scripts/Pathfinder.cs b/Pathfinding Visualizer/Assets/Scripts/Pathfinder.cs index 9d5332c..3085868 100644 --- a/Pathfinding Visualizer/Assets/Scripts/Pathfinder.cs +++ b/Pathfinding Visualizer/Assets/Scripts/Pathfinder.cs @@ -1,8 +1,8 @@ -using System.Collections; +using System.Collections; using System.Collections.Generic; using UnityEngine; using System.Linq; - +using System; public class Pathfinder { @@ -21,7 +21,7 @@ public class Pathfinder public static int height; public static int width; - public Stack result; + public static Stack result; public Pathfinder(LogicGrid world, Vector2Int a, List o, List s, int algo, int heu) @@ -37,7 +37,10 @@ public class Pathfinder algorithm = algo; heuristic = heu; - printInfo(); + //printInfo(); + + + StartWork(); @@ -102,6 +105,9 @@ public class Pathfinder if (currentNode.samples.Count() == 0) { Debug.Log("Gottem"); + + //Node n = FindLastSample(open); + while (currentNode.parent != null) { solution.Push(currentNode); @@ -196,3 +202,266 @@ public class Pathfinder return sorted; }*/ } + +public class Node + { + // helper variables just to make indexing arrays easier to read + public static int row = 0; + public static int col = 1; + + 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 double fn; // distance traveled + heuristic value + //bool canSample; // for expansion + + // construct node :) + public Node(Node parent, Vector2Int agent, List samples, char lastMove, int distanceTraveled, double heuristic) + { + this.parent = parent; + this.agent = agent; + this.samples = new List(samples); + this.lastMove = lastMove; + this.distanceTraveled = distanceTraveled; + this.heuristic = heuristic; + fn = distanceTraveled + heuristic; + } + + // determine valid moves and collect children to be sent back to search + public List expand(int depth) + { + List children = new List(); + if (this.distanceTraveled >= depth && depth != -1) + return children; + + // document expansion of node and get ready to collect this node's children + // SampleWorld.expansions++; + Vector2Int onSample = new Vector2Int(); + + // since this state is being currently visited (expanded), put in closed list + Pathfinder.closed.Add(this.getState()); + + //////////////////////////////////// + // BEGIN CHECKING FOR VALID MOVES // + //////////////////////////////////// + + // store coordinates for all potential moves to be checked + Vector2Int up = new Vector2Int( this.agent.x, this.agent.y + 1 ); + Vector2Int down = new Vector2Int( this.agent.x, this.agent.y - 1 ); + Vector2Int left = new Vector2Int( this.agent.x - 1, this.agent.y ); + Vector2Int right = new Vector2Int( this.agent.x + 1, this.agent.y); + + // make sure going up doesn't go outside world-bounds or into obstacle + if (isOpen(up)) + { + // if move is valid, create that new node/state and document + Node child = new Node(this, up, this.samples, 'U', this.distanceTraveled + 1, this.heuristic); + //SampleWorld.nodesGenerated++; + + // make sure that we have not already made that move + if (!child.getState().inClosed()) + children.Add(child); + } + + // same idea but for the different potential moves + if (isOpen(down)) + { + Node child = new Node(this, down, this.samples, 'D', this.distanceTraveled + 1, this.heuristic); + //SampleWorld.nodesGenerated++; + + if (!child.getState().inClosed()) + children.Add(child); + } + + if (isOpen(left)) + { + Node child = new Node(this, left, this.samples, 'L', this.distanceTraveled + 1, this.heuristic); + //SampleWorld.nodesGenerated++; + + if (!child.getState().inClosed()) + children.Add(child); + } + + if (isOpen(right)) + { + Node child = new Node(this, right, this.samples, 'R', this.distanceTraveled + 1, this.heuristic); + //SampleWorld.nodesGenerated++; + + if (!child.getState().inClosed()) + children.Add(child); + } + + // CHECK IF CAN SAMPLE + onSample = CanSample(); + if (onSample.x == 1) + { + Debug.Log("SAMPLING"); + Node child = new Node(this, this.agent, this.samples, 'S', this.distanceTraveled + 1, this.heuristic); + + child.samples.RemoveAt(onSample.y); + + //SampleWorld.nodesGenerated++; + children.Add(child); + } + + return children; + } + + // helper for expand, verifies whether potential move is legal + public bool isOpen(Vector2Int position) + { + // check that agent is not trying to move into an obstacle + for (int i = 0; i < Pathfinder.obstacles.Count(); i++) + { + if (Pathfinder.obstacles[i].x.Equals(position.x) && Pathfinder.obstacles[i].y.Equals(position.y)) + return false; + } + + if ((position.y < 0) || (position.y > Pathfinder.height - 1) || (position.x < 0) || (position.x > Pathfinder.width - 1)) + return false; + + // check that agent is not stepping out of the world + // if (!((position.y >= 0) && (position.y <= Pathfinder.height - 1) && (position.x >= 0) && (position.x <= Pathfinder.width - 1))) + // return false; + + return true; + } + + // returns the coordinates of the sample closest to the agent's current location + public Vector2Int nearestSample() + { + if (samples.Count == 0) + return agent; + + Vector2Int s = samples[0]; + double lowest = distance(agent, samples[0]); + double dist; + + for (int i = 1; i < samples.Count; i++) + { + dist = distance(agent, samples[i]); + if (dist < lowest) + { + lowest = dist; + s = samples[i]; + } + } + return s; + } + // helper function for nearestSample() + public static double distance(Vector2Int a, Vector2Int b) + { // _________________________ + // distance between 2D points = √(y2 - y1)^2 + (x2 - x1)^2 + double total = (((b.y - a.y) * (b.y - a.y)) + ((b.x - a.x) * (b.x - a.x))); + total = Math.Sqrt(total); + + return total; + } + + // helper function for h2 + ///////////////////////////// + // returns two pieces of information if true: + // [0] is 1 to indicate agent can sample + // [1] is the index of the valid sample in the list + public Vector2Int CanSample() + { + // default to false + Vector2Int result = new Vector2Int(); + result.x = 0; + result.y = -1; + + for (int i = 0; i < this.samples.Count; i++) + { + if ((this.agent.y == samples[i].y) && (this.agent.x == samples[i].x)) + { + result[0] = 1; + result[1] = i; + return result; + } + } + return result; + } + + // returns only the dynamic information directly related to the Node's state. + // we need a way to extract just this information for the sake of our closed list + public State getState() + { + return new State(this.agent, this.samples); + } + + // we use to override comparisons for priorityQueues so that our heuristic calculations are actually used +/* public int CompareTo(Node other) + { + if (this.fn > other.fn) + return 1; + else + return -1; + }*/ + + } + + // helper class for Node +/* 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) + { + if (a.fn > b.fn) + return 1; + else + return -1; + } + }*/ + // helper class for Node + public class State + { + Vector2Int agent; + List samples; + + public State(Vector2Int agent, List samples) + { + this.agent = agent; + this.samples = samples; + + // convert List into 2D array of ints [][] + /*for (int i = 0; i < samples.Count; i++) + { + this.samples[i] = samples[i]; + }*/ + } + + public bool inClosed() + { + for (int i = 0; i < Pathfinder.closed.Count(); i++) + { + if (this.equals(Pathfinder.closed[i])) + { + return true; + } + } + return false; + } + + public bool equals(State other) + { + if (this.samples.Count != other.samples.Count) + return false; + + if (!(this.agent.x.Equals(other.agent.x) && this.agent.y.Equals(other.agent.y))) + return false; + + for (int i = 0; i < this.samples.Count; i++) + { + //if (Arrays.compare(this.samples[i], other.samples[i]) != 0) + + if (this.samples.SequenceEqual(other.samples)) + return false; + } + + return true; + } + } \ No newline at end of file