[pax]

Сообщение от mr.DIMAS

Код в студию. Именно код, где на вход дается граф, начало и конец а на выходе массив точек, представляющий путь из начала в конец.

Вообще писал так, чтобы можно было и другие алгоритмы поиска использовать, поэтому есть отдельно Waypoint (на карте) и AStarNode - содержит стоимость пути, эвристику и приоритет точки.

using System;
using System.Collections.Generic;
using UnityEngine;
using System.Collections;

// класс вейпоинта
public class Waypoint
{
        public Vector3 position;
        public List<WayPointLink> nearPoints = new List<WayPointLink>(); // связи
        public int index; // индекс вейпоинта в массиве вейпоинтов
}

// связь с заранее посчитанным расстоянием до соседней точки
public class WayPointLink
{
        public Waypoint waypoint;
        public float distance;
}


public static class AStar
{
    // разные эвристики

    public static float DistanceHeuristic(Waypoint wp1, Waypoint wp2)
    {
        return (wp1.position - wp2.position).magnitude;
    }

    public static float Distance2DHeuristic(Waypoint wp1, Waypoint wp2)
    {
        var vec = wp1.position - wp2.position;
        var vec2 = new Vector2(vec.x, vec.z);
        return vec2.magnitude;
    }

    public static float SqrDistanceHeuristic(Waypoint wp1, Waypoint wp2)
    {
        return (wp1.position - wp2.position).sqrMagnitude;
    }

    public static float SqrDistance2DHeuristic(Waypoint wp1, Waypoint wp2)
    {
        var vec = wp1.position - wp2.position;
        var vec2 = new Vector2(vec.x, vec.z);
        return vec2.sqrMagnitude;
    }

    public static float SumHeuristic(Waypoint wp1, Waypoint wp2)
    {
        var vec = wp1.position - wp2.position;
        return Mathf.Abs(vec.x) + Mathf.Abs(vec.y) + Mathf.Abs(vec.z);
    }

    public static float Sum2DHeuristic(Waypoint wp1, Waypoint wp2)
    {
        var vec = wp1.position - wp2.position;
        return Mathf.Abs(vec.x) + Mathf.Abs(vec.z);
    }


    // собственно функция поиска, получает весь массив точек, начальную и конечную точки
    // возвращает путь
    public static List<Waypoint> FindRoute(List<Waypoint> waypoints, Waypoint start, Waypoint end,
        Func<Waypoint, Waypoint, float> heuristic = null)
    {
        if(heuristic == null)
        {
            heuristic = DistanceHeuristic;
        }

        // открытые поинты, сортированные по приоритету (с учетом пути и эвристики)
        var opendSet = new List<AStarNode>();
        // дополнительная коллекция для быстрого поиска 
        var opendSetById = new Dictionary<int, AStarNode>();

        // уже просмотренные точки
        var closedSet = new Dictionary<int, AStarNode>();

        // стартовая точка
        var startNode = new AStarNode(start);
        startNode.g = 0;
        startNode.h = heuristic(startNode.waypoint, end);
        startNode.f = startNode.g + startNode.h;

        // добавляем ее в список точек для просмотра 
        opendSet.Add(startNode);
        opendSetById.Add(startNode.waypoint.index, startNode);

        // пока есть точки в открытом списке выполняем поиск
        while (opendSet.Count > 0)
        {
            // достаем последнюю точку (ближайшую до целевой)
            var x = opendSet[opendSet.Count - 1];

            // если это конечная точка, то строим маршрут
            if (x.waypoint == end)
            {
                return ConstructRoute(x);
            }

            // переносим точку в список просмотренных
            opendSet.RemoveAt(opendSet.Count - 1);
            opendSetById.Remove(x.waypoint.index);
            closedSet.Add(x.waypoint.index, x);

            // просматриваем все точки, которые рядом с точкой, которую мы обрабатываем 
            foreach (var yWp in x.waypoint.nearPoints)
            {
                // если точка уже просмотрена, то пропускаем
                if (closedSet.ContainsKey(yWp.waypoint.index)) continue;

                // 
                var wp = waypoints[yWp.waypoint.index];
                AStarNode y;

                //расчетная стоимость пути до этой точки из обрабатываемой
                var tentativeGScore = x.g + yWp.distance;
                
                // необходимость изменить родителя просматриваемой соседней точки и пересчитать параметры, 
                // при условии что стоимость пути до нее меньше или при первом добавлении
                var tentativeIsBetter = false;

                // если точна не существует в открытом списке, создаем точку и добавляем в список открытых
                if (!opendSetById.ContainsKey(wp.index))
                {
                    y = new AStarNode(wp);
                    добавляем точку в сортированный список по дистанции
                    InsertSorted(opendSet, y);
                    opendSetById.Add(y.waypoint.index, y);
                    tentativeIsBetter = true;
                }
                else 
                {
                    // точка уже добавлена в открытый список
                    y = opendSetById[wp.index];
                    // если стоимость меньше, то меняем родительскую точку и пересчитываем параметры
                    if (tentativeGScore < y.g) 
                    {
                        tentativeIsBetter = true;
                    }
                }

                if (!tentativeIsBetter) continue;


                // обновление точки
                y.from = x;
                y.g = tentativeGScore; // стоимость пути
                y.h = heuristic(y.waypoint, end); // эвристика данной точки
                y.f = y.g + y.h; // приоритет 

                // сортировка массива по приоритету
                UpdateSorted(opendSet, y);
            }

        }
        return null;
    }

    // функция перемещает точку по массиву используя ее приоритет (не сортирует остальные точки)
    // писал давно, не помню точно как работает, но наиболее приортитетные точки в конце списка,
    // из конца дешевле убирать точки, не придется сдвигать весь массив
    private static void UpdateSorted(List<AStarNode> opendSet, AStarNode aStarNode)
    {
        var index = opendSet.IndexOf(aStarNode);
        if (index >= 0)
        {
            var isDown = false;
            for (int i = index + 1; i < opendSet.Count; ++i)
            {
                if (opendSet[i].f >= opendSet[i - 1].f)
                {
                    var temp = opendSet[i];
                    opendSet[i] = opendSet[i - 1];
                    opendSet[i - 1] = temp;
                    isDown = true;
                }
                else
                {
                    break;
                }
            }

            if (!isDown)
            {
                for (int i = index - 1; i > -1; --i)
                {
                    if (opendSet[i + 1].f < opendSet[i].f)
                    {
                        break;
                    }

                    var temp = opendSet[i];
                    opendSet[i] = opendSet[i + 1];
                    opendSet[i + 1] = temp;
                }
            }
        }
        else
        {
            // не существует элемента в списке
        }
    }

    // добавляет точку в массив открытых точек в позицию с учетом приоритета
    private static void InsertSorted(List<AStarNode> opendSet, AStarNode aStarNode)
    {
        for (int i = 0; i < opendSet.Count; ++i)
        {
            if (!(aStarNode.f < opendSet[i].f)) continue;

            opendSet.Insert(i, aStarNode);
            return;
        }

        opendSet.Add(aStarNode);
    }

    // функция строит путь
    private static List<Waypoint> ConstructRoute(AStarNode end)
    {
        var route = new List<Waypoint>();

        var wp = end;
        route.Add(wp.waypoint);
        while (wp.from != null && wp != wp.from)
        {
            wp = wp.from;
            route.Add(wp.waypoint);
        }
        if (route.Count>0)
            route.RemoveAt(route.Count-1);
        route.Reverse();

        return route;
    }

    public class AStarNode
    {
        public readonly Waypoint waypoint; // связь с точкой на карте
        public float g; // стоимость
        public float h; // эвристика
        public float f;  // приоритет

        public AStarNode from; // откуда в эту точку посчитана стоимость

        public AStarNode(Waypoint wp)
        {
            waypoint = wp;
        }
    }
}