Skip to content

Commit 1dabc7d

Browse files
SebLagueSebastian Lague
SebLague
authored and
Sebastian Lague
committed
A* Pathfinding in Unity2D
1 parent 8b439f4 commit 1dabc7d

5 files changed

Lines changed: 426 additions & 0 deletions

File tree

‎Pathfinding 2D/Grid.cs‎

Lines changed: 91 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,91 @@
1+
using UnityEngine;
2+
using System.Collections;
3+
using System.Collections.Generic;
4+
5+
public class Grid : MonoBehaviour {
6+
7+
public bool displayGridGizmos;
8+
9+
public LayerMask unwalkableMask;
10+
public Vector2 gridWorldSize;
11+
public float nodeRadius;
12+
13+
Node[,] grid;
14+
float nodeDiameter;
15+
int gridSizeX, gridSizeY;
16+
17+
void Awake() {
18+
nodeDiameter = nodeRadius*2;
19+
gridSizeX = Mathf.RoundToInt(gridWorldSize.x/nodeDiameter);
20+
gridSizeY = Mathf.RoundToInt(gridWorldSize.y/nodeDiameter);
21+
22+
CreateGrid();
23+
}
24+
25+
public int MaxSize {
26+
get {
27+
return gridSizeX * gridSizeY;
28+
}
29+
}
30+
31+
void CreateGrid() {
32+
grid = new Node[gridSizeX,gridSizeY];
33+
Vector2 worldBottomLeft = (Vector2)transform.position - Vector2.right * gridWorldSize.x/2 - Vector2.up * gridWorldSize.y/2;
34+
35+
for (int x = 0; x < gridSizeX; x ++) {
36+
for (int y = 0; y < gridSizeY; y ++) {
37+
Vector2 worldPoint = worldBottomLeft + Vector2.right * (x * nodeDiameter + nodeRadius) + Vector2.up * (y * nodeDiameter + nodeRadius);
38+
bool walkable = (Physics2D.OverlapCircle(worldPoint,nodeRadius,unwalkableMask) == null); // if no collider2D is returned by overlap circle, then this node is walkable
39+
40+
grid[x,y] = new Node(walkable,worldPoint, x,y);
41+
}
42+
}
43+
}
44+
45+
46+
public List<Node> GetNeighbours(Node node, int depth = 1) {
47+
List<Node> neighbours = new List<Node>();
48+
49+
for (int x = -depth; x <= depth; x++) {
50+
for (int y = -depth; y <= depth; y++) {
51+
if (x == 0 && y == 0)
52+
continue;
53+
54+
int checkX = node.gridX + x;
55+
int checkY = node.gridY + y;
56+
57+
if (checkX >= 0 && checkX < gridSizeX && checkY >= 0 && checkY < gridSizeY) {
58+
neighbours.Add(grid[checkX,checkY]);
59+
}
60+
}
61+
}
62+
63+
return neighbours;
64+
}
65+
66+
67+
public Node NodeFromWorldPoint(Vector2 worldPosition) {
68+
float percentX = (worldPosition.x + gridWorldSize.x/2) / gridWorldSize.x;
69+
float percentY = (worldPosition.y + gridWorldSize.y/2) / gridWorldSize.y;
70+
percentX = Mathf.Clamp01(percentX);
71+
percentY = Mathf.Clamp01(percentY);
72+
73+
int x = Mathf.RoundToInt((gridSizeX-1) * percentX);
74+
int y = Mathf.RoundToInt((gridSizeY-1) * percentY);
75+
return grid[x,y];
76+
}
77+
78+
void OnDrawGizmos() {
79+
Gizmos.DrawWireCube(transform.position,new Vector2(gridWorldSize.x,gridWorldSize.y));
80+
if (grid != null && displayGridGizmos) {
81+
foreach (Node n in grid) {
82+
Gizmos.color = Color.red;
83+
if (n.walkable)
84+
Gizmos.color = Color.white;
85+
86+
Gizmos.DrawCube(n.worldPosition, Vector3.one * (nodeDiameter-.1f));
87+
}
88+
}
89+
}
90+
91+
}

‎Pathfinding 2D/Heap.cs‎

Lines changed: 107 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,107 @@
1+
using UnityEngine;
2+
using System.Collections;
3+
using System;
4+
5+
public class Heap<T> where T : IHeapItem<T> {
6+
7+
T[] items;
8+
int currentItemCount;
9+
10+
public Heap(int maxHeapSize) {
11+
items = new T[maxHeapSize];
12+
}
13+
14+
public void Add(T item) {
15+
item.HeapIndex = currentItemCount;
16+
items[currentItemCount] = item;
17+
SortUp(item);
18+
currentItemCount++;
19+
}
20+
21+
public T RemoveFirst() {
22+
T firstItem = items[0];
23+
currentItemCount--;
24+
items[0] = items[currentItemCount];
25+
items[0].HeapIndex = 0;
26+
SortDown(items[0]);
27+
return firstItem;
28+
}
29+
30+
public void UpdateItem(T item) {
31+
SortUp(item);
32+
}
33+
34+
public int Count {
35+
get {
36+
return currentItemCount;
37+
}
38+
}
39+
40+
public bool Contains(T item) {
41+
return Equals(items[item.HeapIndex], item);
42+
}
43+
44+
void SortDown(T item) {
45+
while (true) {
46+
int childIndexLeft = item.HeapIndex * 2 + 1;
47+
int childIndexRight = item.HeapIndex * 2 + 2;
48+
int swapIndex = 0;
49+
50+
if (childIndexLeft < currentItemCount) {
51+
swapIndex = childIndexLeft;
52+
53+
if (childIndexRight < currentItemCount) {
54+
if (items[childIndexLeft].CompareTo(items[childIndexRight]) < 0) {
55+
swapIndex = childIndexRight;
56+
}
57+
}
58+
59+
if (item.CompareTo(items[swapIndex]) < 0) {
60+
Swap (item,items[swapIndex]);
61+
}
62+
else {
63+
return;
64+
}
65+
66+
}
67+
else {
68+
return;
69+
}
70+
71+
}
72+
}
73+
74+
void SortUp(T item) {
75+
int parentIndex = (item.HeapIndex-1)/2;
76+
77+
while (true) {
78+
T parentItem = items[parentIndex];
79+
if (item.CompareTo(parentItem) > 0) {
80+
Swap (item,parentItem);
81+
}
82+
else {
83+
break;
84+
}
85+
86+
parentIndex = (item.HeapIndex-1)/2;
87+
}
88+
}
89+
90+
void Swap(T itemA, T itemB) {
91+
items[itemA.HeapIndex] = itemB;
92+
items[itemB.HeapIndex] = itemA;
93+
int itemAIndex = itemA.HeapIndex;
94+
itemA.HeapIndex = itemB.HeapIndex;
95+
itemB.HeapIndex = itemAIndex;
96+
}
97+
98+
99+
100+
}
101+
102+
public interface IHeapItem<T> : IComparable<T> {
103+
int HeapIndex {
104+
get;
105+
set;
106+
}
107+
}

‎Pathfinding 2D/Node.cs‎

Lines changed: 45 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
1+
using UnityEngine;
2+
using System.Collections;
3+
4+
public class Node : IHeapItem<Node> {
5+
6+
public bool walkable;
7+
public Vector2 worldPosition;
8+
public int gridX;
9+
public int gridY;
10+
11+
public int gCost;
12+
public int hCost;
13+
public Node parent;
14+
int heapIndex;
15+
16+
public Node(bool _walkable, Vector2 _worldPos, int _gridX, int _gridY) {
17+
walkable = _walkable;
18+
worldPosition = _worldPos;
19+
gridX = _gridX;
20+
gridY = _gridY;
21+
}
22+
23+
public int fCost {
24+
get {
25+
return gCost + hCost;
26+
}
27+
}
28+
29+
public int HeapIndex {
30+
get {
31+
return heapIndex;
32+
}
33+
set {
34+
heapIndex = value;
35+
}
36+
}
37+
38+
public int CompareTo(Node nodeToCompare) {
39+
int compare = fCost.CompareTo(nodeToCompare.fCost);
40+
if (compare == 0) {
41+
compare = hCost.CompareTo(nodeToCompare.hCost);
42+
}
43+
return -compare;
44+
}
45+
}

‎Pathfinding 2D/Pathfinding.cs‎

Lines changed: 131 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,131 @@
1+
using UnityEngine;
2+
using System.Collections;
3+
using System.Collections.Generic;
4+
using System.Diagnostics;
5+
using System;
6+
7+
public class Pathfinding : MonoBehaviour {
8+
9+
Grid grid;
10+
static Pathfinding instance;
11+
12+
void Awake() {
13+
grid = GetComponent<Grid>();
14+
instance = this;
15+
}
16+
17+
public static Vector2[] RequestPath(Vector2 from, Vector2 to) {
18+
return instance.FindPath (from, to);
19+
}
20+
21+
Vector2[] FindPath(Vector2 from, Vector2 to) {
22+
23+
Stopwatch sw = new Stopwatch();
24+
sw.Start();
25+
26+
Vector2[] waypoints = new Vector2[0];
27+
bool pathSuccess = false;
28+
29+
Node startNode = grid.NodeFromWorldPoint(from);
30+
Node targetNode = grid.NodeFromWorldPoint(to);
31+
startNode.parent = startNode;
32+
33+
34+
if (startNode.walkable && targetNode.walkable) {
35+
Heap<Node> openSet = new Heap<Node>(grid.MaxSize);
36+
HashSet<Node> closedSet = new HashSet<Node>();
37+
openSet.Add(startNode);
38+
39+
while (openSet.Count > 0) {
40+
Node currentNode = openSet.RemoveFirst();
41+
closedSet.Add(currentNode);
42+
43+
if (currentNode == targetNode) {
44+
sw.Stop();
45+
print ("Path found: " + sw.ElapsedMilliseconds + " ms");
46+
pathSuccess = true;
47+
break;
48+
}
49+
50+
foreach (Node neighbour in grid.GetNeighbours(currentNode)) {
51+
if (!neighbour.walkable || closedSet.Contains(neighbour)) {
52+
continue;
53+
}
54+
55+
int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour)+TurningCost(currentNode,neighbour);
56+
if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) {
57+
neighbour.gCost = newMovementCostToNeighbour;
58+
neighbour.hCost = GetDistance(neighbour, targetNode);
59+
neighbour.parent = currentNode;
60+
61+
if (!openSet.Contains(neighbour))
62+
openSet.Add(neighbour);
63+
else
64+
openSet.UpdateItem(neighbour);
65+
}
66+
}
67+
}
68+
}
69+
70+
if (pathSuccess) {
71+
waypoints = RetracePath(startNode,targetNode);
72+
}
73+
74+
return waypoints;
75+
76+
}
77+
78+
79+
int TurningCost(Node from, Node to) {
80+
return 0;
81+
Vector2 dirOld = new Vector2(from.gridX - from.parent.gridX, from.gridY - from.parent.gridY);
82+
Vector2 dirNew = new Vector2(to.gridX - from.gridX, to.gridY - from.gridY);
83+
if (dirNew == dirOld)
84+
return 0;
85+
else if (dirOld.x != 0 && dirOld.y != 0 && dirNew.x != 0 && dirNew.y != 0) {
86+
return 5;
87+
}
88+
else {
89+
return 10;
90+
}
91+
}
92+
93+
Vector2[] RetracePath(Node startNode, Node endNode) {
94+
List<Node> path = new List<Node>();
95+
Node currentNode = endNode;
96+
97+
while (currentNode != startNode) {
98+
path.Add(currentNode);
99+
currentNode = currentNode.parent;
100+
}
101+
Vector2[] waypoints = SimplifyPath(path);
102+
Array.Reverse(waypoints);
103+
return waypoints;
104+
105+
}
106+
107+
Vector2[] SimplifyPath(List<Node> path) {
108+
List<Vector2> waypoints = new List<Vector2>();
109+
Vector2 directionOld = Vector2.zero;
110+
111+
for (int i = 1; i < path.Count; i ++) {
112+
Vector2 directionNew = new Vector2(path[i-1].gridX - path[i].gridX,path[i-1].gridY - path[i].gridY);
113+
if (directionNew != directionOld) {
114+
waypoints.Add(path[i].worldPosition);
115+
}
116+
directionOld = directionNew;
117+
}
118+
return waypoints.ToArray();
119+
}
120+
121+
int GetDistance(Node nodeA, Node nodeB) {
122+
int dstX = Mathf.Abs(nodeA.gridX - nodeB.gridX);
123+
int dstY = Mathf.Abs(nodeA.gridY - nodeB.gridY);
124+
125+
if (dstX > dstY)
126+
return 14*dstY + 10* (dstX-dstY);
127+
return 14*dstX + 10 * (dstY-dstX);
128+
}
129+
130+
131+
}

0 commit comments

Comments
 (0)