A Star算法笔记

回顾A*算法，偶得一源代码，略有瑕疵，改正之，并置于下。

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

namespace AStarOne
{
    class AStar
    {
        public const int OBLIQUE = 14;//sqrt(2.0) is 1.414; they have been amplified.
        public const int STEP = 10;
        public int[,] AStarArray { get; private set; }
        List<Point> CloseList; // Close List
        List<Point> OpenList; // Open List

        public AStar(int [,] aStar)
        {
            this.AStarArray = aStar;
            OpenList = new List<Point>(AStarArray.Length);// Impossible to be bigger than the number of all data
            CloseList = new List<Point>(AStarArray.Length);
        }

        /// <summary>
        /// Find an achievable path from start to end
        /// </summary>
        /// <param name="start"></param>
        /// <param name="end"></param>
        /// <param name="IsIgnoreCorner">If ignore diagonal spot</param>
        /// <returns></returns>
        public Point FindPath(Point start, Point end, bool IsIgnoreCorner)
        {
            OpenList.Add(start);
            while (0 != OpenList.Count)
            {
                var tempStart = OpenList.MinPoint();// get the minimum F
                OpenList.RemoveAt(0);
                CloseList.Add(tempStart);

                var surroundPoints = GetSurroundPoints(tempStart, IsIgnoreCorner);// Get surrounding points
                foreach (var point in surroundPoints)
                {
                    if (OpenList.Exists(point))// If existing in the open list, choose the minimum G between tempStart and point; Update
                    {
                        FoundPoint(tempStart, point);
                    }
                    else
                    {
                        NotFoundPoint(tempStart, end, point);
                    }
                }

                if (OpenList.Get(end) != null)
                    return OpenList.Get(end);
            }

            return OpenList.Get(end);
        }

        private void FoundPoint(Point tempStart, Point point)
        {
            var G = CalcG(tempStart, point);
            if (G < point.G)// the minimum one
            {
                point.ParentPoint = tempStart;
                point.G = G;
                point.CalcF();
            }
        }

        private void NotFoundPoint(Point tempPoint, Point end, Point point)
        {
            point.ParentPoint = tempPoint;
            point.G = CalcG(tempPoint, point);
            point.H = CalcH(end, point);
            point.CalcF();
            OpenList.Add(point);// This is quite important
        }

        private int CalcG(Point start, Point point)// Calc the cost from start to point
        {
            int G = (Math.Abs(point.X - start.X) + Math.Abs(point.Y - start.Y)) == 1 ? STEP : OBLIQUE;// Should be 1
            int parentG = point.ParentPoint != null ? point.ParentPoint.G : 0;
            return G + parentG;
        }

        private int CalcH(Point end, Point point)// Estimate the cost to reach the target
        {
            int step = Math.Abs(point.X - end.X) + Math.Abs(point.Y - end.Y);
            return step * STEP;
        }

        public List<Point> GetSurroundPoints(Point point, bool IsIgnoreCorner)
        {
            var surroundPoints = new List<Point>(9);

            for (int x = point.X - 1; x <= point.X + 1; x++)
            {
                for (int y = point.Y - 1; y <= point.Y + 1; y++)
                {
                    if (CanReach(point, x, y, IsIgnoreCorner))
                        surroundPoints.Add(x, y);
                }
            }

            return surroundPoints;
        }

        private bool CanReach(int x, int y)
        {
            return AStarArray[x, y] == 0;
        }

        public bool CanReach(Point start, int x, int y, bool IsIgnoreCorner)
        {
            if (!CanReach(x, y) || CloseList.Exists(x, y))// Cannot reach or has been handled 
            {
                return false;
            }
            else
            {
                if (Math.Abs(x - start.X) + Math.Abs(y - start.Y) == 1)// Adjacent but not diagonal
                {
                    return true;
                }
                else
                {
                    if (CanReach(Math.Abs(x - 1), y) && CanReach(x, Math.Abs(y - 1)))// Make sure diagnonal but not necessary
                    {
                        return IsIgnoreCorner;
                    }
                    else
                    {
                        return false;
                    }
                }
            }
        }
    }

    public class Point
    {
        public Point ParentPoint { get; set; }
        public int F { get; set; } // F = G + H
        public int G { get; set; }
        public int H { get; set; }
        public int X { get; set; }
        public int Y { get; set; }

        public Point(int x, int y)
        {
            this.X = x;
            this.Y = y;
        }

        public void CalcF()
        {
            this.F = this.G + this.H;
        }
    }

    public static class ListHelper
    {
        public static bool Exists(this List<Point> points, Point point)
        {
            foreach (var p in points)
                if ((p.X == point.X) && (p.Y == point.Y))
                    return true;

             return false;
        }

        public static bool Exists(this List<Point> points, int x, int y)
        {
            foreach (var p in points)
                if ((p.X == x) && (p.Y == y))
                    return true;

            return false;
        }

        public static Point MinPoint(this List<Point> points)
        {
            points = points.OrderBy(p => p.F).ToList();
            return points[0];
        }

        public static void Add(this List<Point> points, int x, int y)
        {
            points.Add(new Point(x, y));
        }

        public static Point Get(this List<Point> points, Point point)
        {
            foreach (Point p in points)
                if ((p.X == point.X) && (p.Y == point.Y))
                    return p;

            return null;
        }

        public static void Remove(this List<Point> points, int x, int y)
        {
            foreach (var point in points)
                if ((point.X == x) && (point.Y == y))
                    points.Remove(point);
        }
    }
}

测试代码如下：

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

namespace AStarOne
{
    class Program
    {
        static void Main(string[] args)
        {
            int[,] array = {
                               { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
                               { 1, 0, 1, 1, 1, 0, 1, 0, 0, 0, 0, 1},
                               { 1, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1},
                               { 1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1},
                               { 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 0, 1},
                               { 1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1},
                               { 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 1},
                               { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}
                           };

            AStar astar = new AStar(array);

            Point start = new Point(1, 1);
            Point end = new Point(6, 10);
            var parent = astar.FindPath(start, end, false);

            Console.WriteLine("Print path:");
            while (parent != null)
            {
                //Console.WriteLine(parent.X + ", " + parent.Y);
                array[parent.X, parent.Y] = 8;
                parent = parent.ParentPoint;
            }

            for (int i = 0; i < 8; i++)
            {
                for (int j = 0; j < 12; j++)
                {
                    Console.Write(array[i,j] + " ");
                }
                Console.WriteLine();
            }
        }
    }
}

运行结果如下（注意‘8’的位置即是路径）：