A star算法笔记

  1 using System;
  2 using System.Collections.Generic;
  3 using System.Linq;
  4 using System.Text;
  5 using System.Threading.Tasks;
  6 
  7 namespace AStarOne
  8 {
  9     class AStar
 10     {
 11         public const int OBLIQUE = 14;//sqrt(2.0) is 1.414; they have been amplified.
 12         public const int STEP = 10;
 13         public int[,] AStarArray { get; private set; }
 14         List<Point> CloseList; // Close List
 15         List<Point> OpenList; // Open List
 16 
 17         public AStar(int [,] aStar)
 18         {
 19             this.AStarArray = aStar;
 20             OpenList = new List<Point>(AStarArray.Length);// Impossible to be bigger than the number of all data
 21             CloseList = new List<Point>(AStarArray.Length);
 22         }
 23 
 24         /// <summary>
 25         /// Find an achievable path from start to end
 26         /// </summary>
 27         /// <param name="start"></param>
 28         /// <param name="end"></param>
 29         /// <param name="IsIgnoreCorner">If ignore diagonal spot</param>
 30         /// <returns></returns>
 31         public Point FindPath(Point start, Point end, bool IsIgnoreCorner)
 32         {
 33             OpenList.Add(start);
 34             while (0 != OpenList.Count)
 35             {
 36                 var tempStart = OpenList.MinPoint();// get the minimum F
 37                 OpenList.RemoveAt(0);
 38                 CloseList.Add(tempStart);
 39 
 40                 var surroundPoints = GetSurroundPoints(tempStart, IsIgnoreCorner);// Get surrounding points
 41                 foreach (var point in surroundPoints)
 42                 {
 43                     if (OpenList.Exists(point))// If existing in the open list, choose the minimum G between tempStart and point; Update
 44                     {
 45                         FoundPoint(tempStart, point);
 46                     }
 47                     else
 48                     {
 49                         NotFoundPoint(tempStart, end, point);
 50                     }
 51                 }
 52 
 53                 if (OpenList.Get(end) != null)
 54                     return OpenList.Get(end);
 55             }
 56 
 57             return OpenList.Get(end);
 58         }
 59 
 60         private void FoundPoint(Point tempStart, Point point)
 61         {
 62             var G = CalcG(tempStart, point);
 63             if (G < point.G)// the minimum one
 64             {
 65                 point.ParentPoint = tempStart;
 66                 point.G = G;
 67                 point.CalcF();
 68             }
 69         }
 70 
 71         private void NotFoundPoint(Point tempPoint, Point end, Point point)
 72         {
 73             point.ParentPoint = tempPoint;
 74             point.G = CalcG(tempPoint, point);
 75             point.H = CalcH(end, point);
 76             point.CalcF();
 77             OpenList.Add(point);// This is quite important
 78         }
 79 
 80         private int CalcG(Point start, Point point)// Calc the cost from start to point
 81         {
 82             int G = (Math.Abs(point.X - start.X) + Math.Abs(point.Y - start.Y)) == 1 ? STEP : OBLIQUE;// Should be 1
 83             int parentG = point.ParentPoint != null ? point.ParentPoint.G : 0;
 84             return G + parentG;
 85         }
 86 
 87         private int CalcH(Point end, Point point)// Estimate the cost to reach the target
 88         {
 89             int step = Math.Abs(point.X - end.X) + Math.Abs(point.Y - end.Y);
 90             return step * STEP;
 91         }
 92 
 93         public List<Point> GetSurroundPoints(Point point, bool IsIgnoreCorner)
 94         {
 95             var surroundPoints = new List<Point>(9);
 96 
 97             for (int x = point.X - 1; x <= point.X + 1; x++)
 98             {
 99                 for (int y = point.Y - 1; y <= point.Y + 1; y++)
100                 {
101                     if (CanReach(point, x, y, IsIgnoreCorner))
102                         surroundPoints.Add(x, y);
103                 }
104             }
105 
106             return surroundPoints;
107         }
108 
109         private bool CanReach(int x, int y)
110         {
111             return AStarArray[x, y] == 0;
112         }
113 
114         public bool CanReach(Point start, int x, int y, bool IsIgnoreCorner)
115         {
116             if (!CanReach(x, y) || CloseList.Exists(x, y))// Cannot reach or has been handled 
117             {
118                 return false;
119             }
120             else
121             {
122                 if (Math.Abs(x - start.X) + Math.Abs(y - start.Y) == 1)// Adjacent but not diagonal
123                 {
124                     return true;
125                 }
126                 else
127                 {
128                     if (CanReach(Math.Abs(x - 1), y) && CanReach(x, Math.Abs(y - 1)))// Make sure diagnonal but not necessary
129                     {
130                         return IsIgnoreCorner;
131                     }
132                     else
133                     {
134                         return false;
135                     }
136                 }
137             }
138         }
139     }
140 
141     public class Point
142     {
143         public Point ParentPoint { get; set; }
144         public int F { get; set; } // F = G + H
145         public int G { get; set; }
146         public int H { get; set; }
147         public int X { get; set; }
148         public int Y { get; set; }
149 
150         public Point(int x, int y)
151         {
152             this.X = x;
153             this.Y = y;
154         }
155 
156         public void CalcF()
157         {
158             this.F = this.G + this.H;
159         }
160     }
161 
162     public static class ListHelper
163     {
164         public static bool Exists(this List<Point> points, Point point)
165         {
166             foreach (var p in points)
167                 if ((p.X == point.X) && (p.Y == point.Y))
168                     return true;
169 
170              return false;
171         }
172 
173         public static bool Exists(this List<Point> points, int x, int y)
174         {
175             foreach (var p in points)
176                 if ((p.X == x) && (p.Y == y))
177                     return true;
178 
179             return false;
180         }
181 
182         public static Point MinPoint(this List<Point> points)
183         {
184             points = points.OrderBy(p => p.F).ToList();
185             return points[0];
186         }
187 
188         public static void Add(this List<Point> points, int x, int y)
189         {
190             points.Add(new Point(x, y));
191         }
192 
193         public static Point Get(this List<Point> points, Point point)
194         {
195             foreach (Point p in points)
196                 if ((p.X == point.X) && (p.Y == point.Y))
197                     return p;
198 
199             return null;
200         }
201 
202         public static void Remove(this List<Point> points, int x, int y)
203         {
204             foreach (var point in points)
205                 if ((point.X == x) && (point.Y == y))
206                     points.Remove(point);
207         }
208     }
209 }

 1 using System;
 2 using System.Collections.Generic;
 3 using System.Linq;
 4 using System.Text;
 5 using System.Threading.Tasks;
 6 
 7 namespace AStarOne
 8 {
 9     class Program
10     {
11         static void Main(string[] args)
12         {
13             int[,] array = {
14                                { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
15                                { 1, 0, 1, 1, 1, 0, 1, 0, 0, 0, 0, 1},
16                                { 1, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1},
17                                { 1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1},
18                                { 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 0, 1},
19                                { 1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1},
20                                { 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 1},
21                                { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}
22                            };
23 
24             AStar astar = new AStar(array);
25 
26             Point start = new Point(1, 1);
27             Point end = new Point(6, 10);
28             var parent = astar.FindPath(start, end, false);
29 
30             Console.WriteLine("Print path:");
31             while (parent != null)
32             {
33                 //Console.WriteLine(parent.X + ", " + parent.Y);
34                 array[parent.X, parent.Y] = 8;
35                 parent = parent.ParentPoint;
36             }
37 
38             for (int i = 0; i < 8; i++)
39             {
40                 for (int j = 0; j < 12; j++)
41                 {
42                     Console.Write(array[i,j] + " ");
43                 }
44                 Console.WriteLine();
45             }
46         }
47     }
48 }