AStarReference.cs

AStarReference.cs

public class AStarReference
{
    public class PathNode
    {
        public Vector3Int coord;
        public float f;
        public float g;
        public float h;
    }

    public class PathQuery
    {
        public List<PathNode> result;
        public List<PathNode> open;
        public List<PathNode> closed;
        public Vector3Int start;
        public Vector3Int end;
    }

    private void Process(Query query)
    {
        var steps = 100000;

        if (query.done)
            return;

        if (query._pathQuery == null)
        {
            query._pathQuery = new PathQuery();
            query._pathQuery.result = new List<PathNode>();
            query._pathQuery.open = new List<PathNode>();
            query._pathQuery.closed = new List<PathNode>();
            query._pathQuery.start = WorldGrid.Instance.grid.WorldToCell(query.src);
            query._pathQuery.end = WorldGrid.Instance.grid.WorldToCell(query.dst);

            query._pathQuery.open.Add(new PathNode() { coord = query._pathQuery.start, });
        }

        var q = query._pathQuery;
        var counter = 0;

        while (q.open.Count > 0)
        {
            if (counter++ >= steps)
                break;

            var current = q.open[q.open.Count - 1];
            if (current.coord == q.end)
            {
                query.done = true;
                break;
            }

            q.open.RemoveAt(q.open.Count - 1);
            q.closed.Add(current);

            var successors = new List<PathNode>();

            // replace with suitable adjacent scan
            var adjacent0 = current.coord + new Vector3Int(+1, 0, 0);
            var adjacent1 = current.coord + new Vector3Int(-1, 0, 0);
            var adjacent2 = current.coord + new Vector3Int(0, +1, 0);
            var adjacent3 = current.coord + new Vector3Int(0, -1, 0);

            var valid0 = true;
            var valid1 = true;
            var valid2 = true;
            var valid3 = true;

            // replace with suitable wall checks
            if (WorldGrid.Instance.outdoorWall.HasTile(adjacent0)) valid0 = false;
            if (WorldGrid.Instance.outdoorWall.HasTile(adjacent1)) valid1 = false;
            if (WorldGrid.Instance.outdoorWall.HasTile(adjacent2)) valid2 = false;
            if (WorldGrid.Instance.outdoorWall.HasTile(adjacent3)) valid3 = false;

            var cost0 = PathfinderWorld.Instance.GetCostCell(adjacent0);
            var cost1 = PathfinderWorld.Instance.GetCostCell(adjacent1);
            var cost2 = PathfinderWorld.Instance.GetCostCell(adjacent2);
            var cost3 = PathfinderWorld.Instance.GetCostCell(adjacent3);

            if (cost0 < 0) valid0 = false;
            if (cost1 < 0) valid1 = false;
            if (cost2 < 0) valid2 = false;
            if (cost3 < 0) valid3 = false;

            // already exists in path
            for (int j = 0; j < q.result.Count; ++j)
            {
                if (q.result[j].coord == adjacent0)
                    valid0 = false;
                if (q.result[j].coord == adjacent1)
                    valid1 = false;
                if (q.result[j].coord == adjacent2)
                    valid2 = false;
                if (q.result[j].coord == adjacent3)
                    valid3 = false;
            }

            if (valid0) successors.Add(new PathNode() { coord = adjacent0, });
            if (valid1) successors.Add(new PathNode() { coord = adjacent1, });
            if (valid2) successors.Add(new PathNode() { coord = adjacent2, });
            if (valid3) successors.Add(new PathNode() { coord = adjacent3, });

            for (int i = 0; i < successors.Count; ++i)
            {
                var child = successors[i];

                var alreadyOpen = false;
                var alreadyClosed = false;

                for (int j = 0; j < q.closed.Count; ++j)
                {
                    if (q.closed[j].coord == child.coord)
                    {
                        alreadyClosed = true;
                        break;
                    }
                }

                if (alreadyClosed)
                    continue;

                var cost = 1.0f;

                child.g = current.g + cost;
                child.h = Vector3Int.Distance(current.coord, q.end);
                child.f = child.g + child.h;

                for (int j = 0; j < q.open.Count; ++j)
                {
                    if (q.open[j].coord == child.coord && current.g > q.open[j].g)
                    {
                        alreadyOpen = true;
                        break;
                    }
                }

                if (alreadyOpen)
                    continue;

                q.open.Add(child);

                // reverse sorted
                q.open.Sort((a, b) =>
                {
                    if (a.f > b.f)
                        return -1;
                    if (a.f < b.f)
                        return +1;
                    return 0;
                });
            }
        }

        if (q.open.Count <= 0)
            query.done = true;

        if (query.done)
        {
            query.path = new List<Vector3Int>();

            for (int i = 0; i < q.result.Count; ++i)
            {
                var index = q.result.Count - i - 1;
                var node = q.result[index];

                query.path.Add(node.coord);
            }
        }
    }
    private void DebugDraw(Query query)
    {
        var q = query._pathQuery;
        if (q == null)
            return;

        var a = WorldGrid.Instance.grid.CellToWorld(q.start);
        var b = WorldGrid.Instance.grid.CellToWorld(q.end);

        Debug.DrawLine(query.src, query.dst, Color.white);
        Debug.DrawLine(a, b, Color.grey);

        for (int i = 0; i < q.open.Count; ++i)
        {
            var o = q.open[i];
            var p = WorldGrid.Instance.grid.CellToWorld(o.coord);

            DebugE.DrawBox2DCentered(p, Vector2.one * 0.3f, Color.green, 0.0f, false);
        }

        for (int i = 0; i < q.closed.Count; ++i)
        {
            var o = q.closed[i];
            var p = WorldGrid.Instance.grid.CellToWorld(o.coord);

            DebugE.DrawBox2DCentered(p, Vector2.one * 0.35f, Color.red, 0.0f, false);
        }
    }
}