LD50/Assets/Scripts/PathFinding.cs

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;
        }
    }
}