Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- using System;
- using System.Collections.Generic;
- using UnityEngine;
- // A* needs only a WeightedGraph and a Vector3 type L, and does *not*
- // have to be a grid. However, in the example code I am using a grid.
- public class SquareGrid
- {
- // Implementation notes: I made the fields public for convenience,
- // but in a real project you'll probably want to follow standard
- // style and make them private.
- public static readonly Vector3[] DIRS = new[]
- {
- new Vector3(1, 0),
- new Vector3(1, 1),
- new Vector3(1, -1),
- new Vector3(-1, 1),
- new Vector3(-1, -1),
- new Vector3(0, -1),
- new Vector3(-1, 0),
- new Vector3(0, 1)
- };
- public int width, height;
- //public HashSet<NodeElement> walls = new HashSet<NodeElement>();
- //public HashSet<NodeElement> forests = new HashSet<NodeElement>();
- public NodeElement[,] nodes;
- public SquareGrid(int width, int height)
- {
- nodes = new NodeElement[width, height];
- for (int x = 0; x < width; x++)
- {
- for (int y = 0; y < height; y++)
- {
- nodes[x, y] = new NodeElement(new Vector3(x, 0, y), 1, true);
- }
- }
- this.width = width;
- this.height = height;
- }
- public NodeElement FindNode(int x, int y)
- {
- if (InBounds(x, y))
- return nodes[x, y];
- return null;
- }
- public bool InBounds(NodeElement id)
- {
- return 0 <= id.x && id.x < width
- && 0 <= id.y && id.y < height;
- }
- public bool InBounds(int x, int y)
- {
- return 0 < x && x < width
- && 0 <= y && y < height;
- }
- public bool Passable(NodeElement id)
- {
- return id.passable && id.occupied == null;//!walls.Contains(id);
- }
- public double Cost(NodeElement a, NodeElement b)
- {
- double c = b.item2;
- if (a.x != b.x && a.y != b.y)
- c += 0.1f;
- return c;
- }
- public IEnumerable<NodeElement> Neighbors(NodeElement id)
- {
- foreach (var dir in DIRS)
- {
- int x = (int)(id.x + dir.x);
- int y = (int)(id.y + dir.y);
- if (InBounds(x, y))
- {
- NodeElement next = FindNode(x, y);
- if (Passable(next))
- {
- yield return next;
- }
- }
- }
- }
- }
- [System.Serializable]
- public class NodeElement
- {
- public int x, y;
- public double item2;
- public bool passable;
- public Seeker occupied;
- public NodeElement cameFrom;
- public double costSofar;
- public bool passed;
- public double priority;
- public MeshRenderer mesh;
- public NodeElement(Vector3 _node, double _double, bool _passable)
- {
- x = (int)_node.x;
- y = (int)_node.z;
- item2 = _double;
- passable = _passable;
- occupied = null;
- costSofar = 0;
- cameFrom = this;
- passed = false;
- priority = 0;
- mesh = null;
- }
- public NodeElement()
- {
- x = 0;
- y = 0;
- item2 = 0;
- passable = false;
- occupied = null;
- costSofar = 0;
- cameFrom = this;
- passed = false;
- priority = 0;
- mesh = null;
- }
- public Vector3 pos()
- {
- return new Vector3(x, 0, y);
- }
- }
- public class PriorityQueue
- {
- // I'm using an unsorted array for this example, but ideally this
- // would be a binary heap. Find a binary heap class:
- // * https://bitbucket.org/BlueRaja/high-speed-priority-queue-for-c/wiki/Home
- // * http://visualstudiomagazine.com/articles/2012/11/01/priority-queues-with-c.aspx
- // * http://xfleury.github.io/graphsearch.html
- // * http://stackoverflow.com/questions/102398/priority-queue-in-net
- private List<NodeElement> elements = new List<NodeElement>();
- public int Count
- {
- get { return elements.Count; }
- }
- public void Enqueue(NodeElement next,double priority)
- {
- next.passed = true;
- next.priority = priority;
- elements.Add(next);
- }
- public NodeElement Dequeue()
- {
- int bestIndex = 0;
- for (int i = 0; i < elements.Count; i++)
- {
- if (elements[i].priority < elements[bestIndex].priority)
- {
- bestIndex = i;
- }
- }
- NodeElement bestItem = elements[bestIndex];
- elements.RemoveAt(bestIndex);
- return bestItem;
- }
- }
- /* NOTE about types: in the main article, in the Python code I just
- * use numbers for costs, heuristics, and priorities. In the C++ code
- * I use a typedef for this, because you might want int or double or
- * another type. In this C# code I use double for costs, heuristics,
- * and priorities. You can use an int if you know your values are
- * always integers, and you can use a smaller size number if you know
- * the values are always small. */
- public class AStarSearch
- {
- /*public Dictionary<NodeElement, NodeElement> cameFrom
- = new Dictionary<NodeElement, NodeElement>();
- public Dictionary<NodeElement, double> costSoFar
- = new Dictionary<NodeElement, double>();
- */
- // Note: a generic version of A* would abstract over Vector3 and
- // also Heuristic
- static public double Heuristic(int xa, int ya, int xb, int yb)
- {
- return Math.Abs(xa - xb) + Math.Abs(ya - yb);
- }
- NodeElement _start, _goal;
- public AStarSearch(SquareGrid graph, NodeElement start, NodeElement goal)
- {
- var frontier = new PriorityQueue();
- frontier.Enqueue(start,0);
- _start = start;
- _goal = goal;
- while (frontier.Count > 0)
- {
- NodeElement current = frontier.Dequeue();
- if (current.Equals(goal))
- {
- break;
- }
- foreach (var next in graph.Neighbors(current))
- {
- double newCost = current.costSofar + graph.Cost(current, next);
- if (!next.passed || (newCost < next.costSofar && next.costSofar > 0))
- {
- next.costSofar = newCost;
- double priority = newCost + Heuristic(next.x, next.y, goal.x, goal.y);
- frontier.Enqueue(next,priority);
- next.cameFrom = current;
- }
- }
- }
- }
- public List<NodeElement> FindShortestPath()
- {
- NodeElement next = _goal;
- List<NodeElement> path = new List<NodeElement>();
- path.Add(next);
- if (next.cameFrom != null)
- {
- for (int i = 0; i < _goal.costSofar; ++i)
- {
- next = next.cameFrom;
- path.Add(next);
- if (next == _start)
- {
- path.Reverse();
- break;
- }
- }
- }
- return path;
- }
- }
Add Comment
Please, Sign In to add comment