Алгоритм Астар заходит в бесконечный цикл

Question

Я работал над алгоритмом поиска A * и следовал этому: A * алгоритм поиска

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using MapData.Interfaces;

namespace MapData.Algorithm {
    public class AStar {
        IDMap map;
        List<Location2D> openset;
        List<Location2D> closedset;
        List<Location2D> path;
        Dictionary<Location2D, Double> g_scores;
        Dictionary<Location2D, Double> h_scores;
        Dictionary<Location2D, Double> f_scores;
        Dictionary<Location2D, Location2D> came_from;
        int[] ArrayX = { 0, -1, -1, -1, 0, 1, 1, 1 };
        int[] ArrayY = { 1, 1, 0, -1, -1, -1, 0, 1 };

        /// <summary>
        /// Initing the Algorithm with MapInfo
        /// </summary>
        /// <param name="map">DMap info.</param>
        public AStar(IDMap map) {
            this.map = map;
        }

        /// <summary>
        /// Start searching untile it's find it's goal or return false.
        /// </summary>
        /// <param name="start">Starting Node.</param>
        /// <param name="goal">Ending Node.</param>
        public bool Find( Location2D start, Location2D goal) {
            openset = new List<Location2D>(); // The set of tentative nodes to be evaluated, initially containing the start node.
            closedset = new List<Location2D>(); // The set of nodes already evaluated.
            path = new List<Location2D>(); // The path result.
            g_scores = new Dictionary<Location2D, Double>(); // Cost from start along best known path.
            h_scores = new Dictionary<Location2D, Double>(); // Estimated cost to get from a node to the goal.
            f_scores = new Dictionary<Location2D, Double>(); // Estimated total cost from start to goal.
            came_from = new Dictionary<Location2D, Location2D>(); // The navigated nodes.


            openset.Add(start);
            g_scores[start] = 0;
            h_scores[start] = GetHValue(start, goal);
            f_scores[start] = g_scores[start] + h_scores[start];

            while (openset.Count > 0) { // while openset is not empty

                Location2D current = CheckBestNode(); //the node in openset having the lowest f_score[] value


                if (current.Equals(goal)) {
                    ReconstructPathRecursive(current);
                    return true;
                }

                Location2D neighbor = new Location2D();

                for (int i = 0; i < 8; i++) { // neighbor nodes.
                    neighbor.X = (ushort)(current.X + ArrayX[i]);
                    neighbor.Y = (ushort)(current.Y + ArrayY[i]);

                    bool tentative_is_better = false;

                    if (closedset.Contains(neighbor)) continue;
                    if (!map.Cells[neighbor.X, neighbor.Y].Walkable) continue;

                    Double tentative_g_score = g_scores[current] + DistanceBetween(current, neighbor);

                    if (!openset.Contains(neighbor)) {
                        openset.Add(neighbor);
                        h_scores[neighbor] = GetHValue(neighbor, goal);
                        tentative_is_better = true;
                    } else if (tentative_g_score < g_scores[neighbor]) {
                        tentative_is_better = true;
                    } else {
                        tentative_is_better = false;
                    }

                    if (tentative_is_better) {
                        if (came_from.ContainsKey(neighbor)) {
                            came_from[neighbor] = current;
                        } else {
                            came_from[neighbor] = current;
                            g_scores[neighbor] = tentative_g_score;
                            f_scores[neighbor] = g_scores[neighbor] + h_scores[neighbor];
                        }
                    }
                }
            }
            return false;
        }

        /// <summary>
        /// Check the best node that has the smallest f value;
        /// </summary>
        /// <returns>The best Node.</returns>
        Location2D CheckBestNode() {
            var BestNode = f_scores.OrderBy(x => x.Value).First().Key;
            f_scores.Remove(BestNode);
            openset.Remove(BestNode);
            closedset.Add(BestNode);
            return BestNode;
        }

        private void ReconstructPathRecursive(Location2D current_node) {
            Location2D temp;
            if (came_from.TryGetValue(current_node, out temp)) {
                ReconstructPathRecursive(temp);
                path.Add(temp);
            } else {
                path.Add(current_node);
            }
        }

        int GetHValue(Location2D start, Location2D goal) {
            int nDx = Math.Abs(start.X - goal.X);
            int nDy = Math.Abs(start.Y - goal.Y);
            if (nDx > nDy)
                return 10 * nDx + 6 * nDy;
            return 10 * nDy + 6 * nDx;
        }

        readonly Double SQRT_2 = Math.Sqrt(2);

        protected  Double DistanceBetween(Location2D inStart, Location2D inEnd) {
            int diffX = Math.Abs(inStart.X - inEnd.X);
            int diffY = Math.Abs(inStart.Y - inEnd.Y);

            switch (diffX + diffY) {
                case 1: return 1;
                case 2: return SQRT_2;
                case 0: return 0;
                default:
                    throw new ApplicationException();
            }
        }

        public List<Location2D> Path {
            get {
                return path;
            }
        }
    }
}

Теперь у меня есть две проблемы:

1. Если я проверил пройденные участки, Астар заходит в бесконечный цикл и никогда не находит ни пути, ни разрыва.

2. Если бы я прокомментировал ту часть, где проверяются найденные участки, которые можно пройтипуть, но не точный, например, если местоположение цели -> (444,444), путь заканчивается в -> (444,443) или (443,444).

Я не вижу, чтоя делаю неправильно здесь, так как я следовал руководству Википедии.

Answer 1

Теперь, поправьте меня, если я ошибаюсь, но это похоже похоже на то, что, глядя на ваш другой код, вы хотите, чтобы операторы if в цикле for, который перебирает соседей текущего узла, выглядели какэто:

if (!closedset.Contains(neighbor)) continue;
if (map.Cells[neighbor.X, neighbor.Y].Walkable) continue;

Не так:

if (closedset.Contains(neighbor)) continue;
if (!map.Cells[neighbor.X, neighbor.Y].Walkable) continue;

Ваш исходный код оценивает соседа только в том случае, если он находится в закрытом списке, и он не доступен для прохождения, что я не думаю, что вы хотите,Обновленный код оценивает соседний узел только в том случае, если он не находится в замкнутом наборе и доступен для прохождения.

Для дальнейшего понимания алгоритма A * и подробного описания его работы, проверьте thisссылка .

РЕДАКТИРОВАТЬ: Вы сказали, что ваш код предоставил неточный путь, то есть путь, который не ведет весь путь до целевого квадрата.Если посмотреть на вашу ReconstructPathRecursive() функцию, кажется, что вы совершили решающую ошибку.Поскольку ваш список содержит родительский элемент каждого пройденного узла, вы получите все, кроме целевого квадрата, потому что он не является родительским для всего.Целевой квадрат должен быть первым квадратом, добавленным в ваш список, прежде чем вы начнете работать в обратном направлении, восстанавливая путь от родителя текущего узла (который является целевым квадратом).

Чтобы исправить это поведение, я бы предложилКогда вы обнаружите, что текущий узел равен целевому узлу, вы сначала добавляете целевой узел в свой список путей, а после этого вызываете функцию ReconstructPathRecursive(), чтобы получить окончательный список.