Exemplo n.º 1
0
    private static List <Vector2Int> CalcPath(Point source, Point target)
    {
        var openList  = new PointList();
        var closeList = new PointList();

        source.g      = 0;
        source.h      = Distance(source, target);
        source.f      = source.g + source.h;
        source.parent = new Point(-1, -1);
        openList.AddElement(source);

        Point step;

        while (openList.Count > 0)
        {
            step = openList.GetElementWithMinF();
            List <Point> neighbors = GetNeighbors(step.x, step.y);
            openList.RemoveElement(step);
            closeList.AddElement(step);

            for (int i = 0; i < neighbors.Count; i++)
            {
                Point cell = neighbors[i];
                if (!map.IsWalkable(cell.x, cell.y))
                {
                    continue;
                }

                if (closeList.HasElement(cell))
                {
                    continue;
                }

                float weight = map[cell.x, cell.y];

                if (openList.HasElement(cell))
                {
                    Point st = openList.GetElement(cell);
                    if (st.g > step.g + MIN_G + weight)
                    {
                        st.g      = step.g + MIN_G + weight;
                        st.h      = Distance(st, target);
                        st.f      = st.g + st.h;
                        st.parent = step;
                        openList.UpdateElement(st);
                    }
                }
                else
                {
                    cell.g      = step.g + MIN_G + weight;
                    cell.h      = Distance(cell, target);
                    cell.f      = cell.g + cell.h;
                    cell.parent = step;
                    openList.AddElement(cell);
                }
            }
            if (target.x == step.x && target.y == step.y)
            {
                break;
            }
        }

        List <Vector2Int> lst = new List <Vector2Int>();

        if (openList.Count == 0)
        {
            return(lst);
        }

        step = closeList.GetElement(target);

        lst.Add(step.GetNewVector());
        while (step.parent != null)
        {
            step = step.parent;
            lst.Insert(0, step.GetNewVector());
        }
        lst.RemoveAt(0);

        return(lst);
    }