You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
231 lines
7.6 KiB
231 lines
7.6 KiB
using System; |
|
using System.Collections; |
|
using System.Collections.Generic; |
|
using System.Numerics; |
|
using UnityEngine; |
|
using Vector2 = UnityEngine.Vector2; |
|
using Vector3 = UnityEngine.Vector3; |
|
|
|
public class PathFinding : MonoBehaviour |
|
{ |
|
private float unitSize = .5f; //unit size |
|
|
|
public int a = 0, b = 0; //Bot left corner of grid |
|
public int width = 10, height = 10; //Grid size |
|
|
|
public bool[,] grid; //Valid path nodes |
|
|
|
public GameObject obst; //Obstacle prefab |
|
|
|
private LinkedList<Vector2Int> p; |
|
|
|
// Start is called before the first frame update |
|
void Start() |
|
{ |
|
Debug.Log("size: " + width + "," + height); |
|
InitPathNodes(); |
|
FindPath(new Vector2(-2, 0), new Vector2(2f, 0)); |
|
} |
|
|
|
// Update is called once per frame |
|
void Update() |
|
{ |
|
} |
|
|
|
void InitPathNodes() |
|
{ |
|
int xCount = (int) Mathf.Floor(width / unitSize); |
|
int yCount = (int) Mathf.Floor(height / unitSize); |
|
grid = new bool[xCount, yCount]; |
|
for (int x = 0; x < xCount; x++) |
|
{ |
|
for (int y = 0; y < yCount; y++) |
|
{ |
|
bool walkable = isUnobstructed(a + x * unitSize, b + y * unitSize); |
|
grid[x, y] = walkable; |
|
} |
|
} |
|
|
|
Debug.Log("fuck: " + (1 << obst.layer)); |
|
Debug.Log("test: " + Physics2D.OverlapPoint(new Vector2(0, 0))); |
|
} |
|
|
|
private bool isUnobstructed(float x, float y) |
|
{ |
|
Vector2 p = new Vector2(x, y); |
|
int mask = LayerMask.GetMask("Obstacle"); |
|
Collider2D overlapPoint = Physics2D.OverlapPoint(p, mask); |
|
bool res = overlapPoint == null; |
|
return res; |
|
} |
|
|
|
float Heuristic(Vector2Int node, Vector2Int goal) |
|
{ |
|
//Return the distance between two points |
|
int dx = node.x - goal.x; |
|
int dy = node.y - goal.y; |
|
return Mathf.Sqrt(dx * dx + dy * dy); |
|
} |
|
|
|
List<Vector2Int> getNeighbours(Vector2Int pos) |
|
{ |
|
List<Vector2Int> neighbours = new List<Vector2Int>(); |
|
for (int x = 0; x < 3; x++) |
|
{ |
|
for (int y = 0; y < 3; y++) |
|
{ |
|
if (x == 1 && y == 1) |
|
continue; |
|
Vector2Int n = new Vector2Int(pos.x + x - 1, pos.y + y - 1); |
|
if (n.x >= 0 && n.x < grid.GetLength(0) && n.y >= 0 && n.y < grid.GetLength(1)) |
|
neighbours.Add(n); |
|
} |
|
} |
|
|
|
return neighbours; |
|
} |
|
|
|
|
|
//Find path with the A* algorithm |
|
public LinkedList<Vector2> FindPath(Vector2 start_f, Vector2 goal_f) |
|
{ |
|
//Check if in bounds |
|
if (start_f.x - a < 0 || start_f.x - a > width || start_f.y - b < 0 || start_f.y - b > height) |
|
{ |
|
Debug.LogError("Start out of bounds"); |
|
return null; |
|
} |
|
|
|
if (goal_f.x - a < 0 || goal_f.x - a > width || goal_f.y - b < 0 || goal_f.y - b > height) |
|
{ |
|
Debug.LogError("Goal out of bounds"); |
|
return null; |
|
} |
|
|
|
bool[,] inset = new bool[grid.GetLength(0), grid.GetLength(1)]; |
|
|
|
//Calculate the grid positions of the two points |
|
Vector2Int start = |
|
new Vector2Int(Mathf.FloorToInt((start_f.x - a) / unitSize), Mathf.FloorToInt((start_f.y - b) / unitSize)); |
|
Vector2Int goal = |
|
new Vector2Int(Mathf.FloorToInt((goal_f.x - a) / unitSize), Mathf.FloorToInt((goal_f.y - b) / unitSize)); |
|
|
|
Debug.Log("start: " + start.x + "," + start.y); |
|
Debug.Log("goal: " + goal.x + "," + goal.y); |
|
|
|
|
|
PriorityQueue<(Vector2Int, float)> openset = |
|
new PriorityQueue<(Vector2Int, float)>((a, b) => a.Item2.CompareTo(b.Item2)); |
|
Dictionary<Vector2Int, Vector2Int> cameFrom = new Dictionary<Vector2Int, Vector2Int>(); |
|
|
|
openset.Insert((start, 0)); |
|
float[,] gScore = new float[grid.GetLength(0), grid.GetLength(1)]; |
|
float[,] fScore = new float[grid.GetLength(0), grid.GetLength(1)]; |
|
for (int x = 0; x < grid.GetLength(0); x++) |
|
{ |
|
for (int y = 0; y < grid.GetLength(1); y++) |
|
{ |
|
gScore[x, y] = fScore[x, y] = Mathf.Infinity; |
|
} |
|
} |
|
|
|
gScore[start.x, start.y] = 0; |
|
fScore[start.x, start.y] = Heuristic(start, goal); |
|
|
|
while (!openset.IsEmpty()) |
|
{ |
|
(Vector2Int, float) current = openset.Extract(); |
|
if (current.Item1 == goal) |
|
{ |
|
Debug.Log("Found path"); |
|
Debug.Log("Path length: " + gScore[current.Item1.x, current.Item1.y]); |
|
Debug.Log("Path: " + cameFrom[current.Item1]); |
|
LinkedList<Vector2Int> path = Backtrack(cameFrom, current.Item1); |
|
p = path; |
|
return ConvertPath(path); |
|
} |
|
|
|
inset[current.Item1.x, current.Item1.y] = true; |
|
List<Vector2Int> neighbours = getNeighbours(current.Item1); |
|
foreach (Vector2Int neighbour in neighbours) |
|
{ |
|
if (!grid[neighbour.x, neighbour.y]) |
|
continue; |
|
float tentativeGScore = gScore[current.Item1.x, current.Item1.y] + 1; |
|
if (tentativeGScore >= gScore[neighbour.x, neighbour.y]) |
|
continue; |
|
cameFrom[neighbour] = current.Item1; |
|
gScore[neighbour.x, neighbour.y] = tentativeGScore; |
|
fScore[neighbour.x, neighbour.y] = gScore[neighbour.x, neighbour.y] + Heuristic(neighbour, goal); |
|
if (!inset[neighbour.x, neighbour.y]) |
|
{ |
|
openset.Insert((neighbour, fScore[neighbour.x, neighbour.y])); |
|
inset[neighbour.x, neighbour.y] = true; |
|
} |
|
} |
|
} |
|
|
|
return null; |
|
} |
|
|
|
private LinkedList<Vector2Int> Backtrack(Dictionary<Vector2Int, Vector2Int> cameFrom, Vector2Int current) |
|
{ |
|
LinkedList<Vector2Int> path = new LinkedList<Vector2Int>(); |
|
int count = 0; |
|
Debug.Log("Current: " + current); |
|
path.AddFirst(current); |
|
while (cameFrom.ContainsKey(current)) |
|
{ |
|
count++; |
|
current = cameFrom[current]; |
|
path.AddFirst(current); |
|
Debug.Log("Came from: " + current); |
|
} |
|
|
|
Debug.Log("Count: " + count); |
|
return path; |
|
} |
|
|
|
private LinkedList<Vector2> ConvertPath(LinkedList<Vector2Int> path) |
|
{ |
|
LinkedList<Vector2> converted = new LinkedList<Vector2>(); |
|
foreach (Vector2Int v in path) |
|
{ |
|
converted.AddFirst(new Vector2(v.x * unitSize + a, v.y * unitSize + b)); |
|
} |
|
|
|
return converted; |
|
} |
|
|
|
private void OnDrawGizmos() |
|
{ |
|
if (grid == null) return; |
|
Gizmos.color = Color.yellow; |
|
|
|
int xCount = (int) Mathf.Floor(width / unitSize); |
|
int yCount = (int) Mathf.Floor(height / unitSize); |
|
|
|
for (int x = 0; x < xCount; x++) |
|
{ |
|
for (int y = 0; y < yCount; y++) |
|
{ |
|
Gizmos.color = Color.yellow; |
|
if (!grid[x, y]) |
|
Gizmos.color = Color.red; |
|
Vector3 pos = new Vector3(a + x * unitSize, b + y * unitSize, 0); |
|
Gizmos.DrawSphere(pos, 0.05f); |
|
} |
|
} |
|
|
|
if (p == null) return; |
|
Gizmos.color = Color.magenta; |
|
var prev = p.First; |
|
for (int i = 0; i < p.Count - 1; i++) |
|
{ |
|
Vector3 pos = new Vector3(prev.Value.x * unitSize + a, prev.Value.y * unitSize + b, 0); |
|
Vector3 target = new Vector3(prev.Next.Value.x * unitSize + a, prev.Next.Value.y * unitSize + b, 0); |
|
Gizmos.DrawLine(pos, target); |
|
prev = prev.Next; |
|
} |
|
} |
|
} |