source: 2013/30/DenisZ/CastleMaster/CastleMaster/CastleMaster/Ai/AStar.cs @ 4635

Revision 4635, 7.2 KB checked in by dezhidki, 6 years ago (diff)

Talletus.

Line 
1using System;
2using System.Collections.Generic;
3using System.Linq;
4using System.Text;
5using CastleMaster.World;
6using CastleMaster.Units.Mobs;
7
8namespace IsometricEngineTest.Ai
9{
10    public class AStar
11    {
12        public const int MOVE_STRAIGHT = 10;
13        public const int MOVE_VERTICAL = 14;
14        public const int MOVE_COSTS_SAVED = MOVE_VERTICAL - 2 * MOVE_STRAIGHT;
15        private readonly double TIRE_FIX;
16
17        private Level level;
18        private Mob mob;
19        private List<Node> open, path;
20        private bool[] tiles;
21        private Node[] nodeMap;
22        private bool isPathFinding = false;
23        private bool canPathFind = true;
24        private int calls;
25        private int xStart, zStart, xEnd, zEnd;
26
27#if DEBUG
28        private TimeSpan totalTime = TimeSpan.Zero;
29#endif
30
31        public AStar(Level level, Mob mob)
32        {
33            this.level = level;
34            this.mob = mob;
35            open = new List<Node>();
36            path = new List<Node>();
37
38            TIRE_FIX = (1.0 + (MOVE_STRAIGHT / 1000.0));
39        }
40
41        public List<Node> Path { get { return path; } }
42
43        public bool IsPathFinding { get { return isPathFinding; } }
44
45        public bool CanFindPath { get { return canPathFind; } }
46
47        public int EstimateCost(int x, int z)
48        {
49            int dx = Math.Abs(x - xEnd);
50            int dz = Math.Abs(z - zEnd);
51
52            int h = MOVE_STRAIGHT * (dx + dz) + MOVE_COSTS_SAVED * Math.Min(dx, dz);
53            h = (int)(h * TIRE_FIX);
54            return h;
55        }
56
57        public void InitializePathFinder(int xStart, int zStart, int xEnd, int zEnd, bool ignoreSolidnessForEnd)
58        {
59            tiles = level.BuildSolidnessTable(mob, ignoreSolidnessForEnd);
60            nodeMap = new Node[level.Width * level.Height];
61            open.Clear();
62            path.Clear();
63            calls = 0;
64
65            if(ignoreSolidnessForEnd)
66                tiles[xEnd + zEnd * level.Width] = false;
67
68#if DEBUG
69            totalTime = TimeSpan.Zero;
70            Console.WriteLine("[START] From: [ " + xStart + ", " + zStart + " ] to : [ " + xEnd + ", " + zEnd + "].");
71#endif
72
73            this.xStart = xStart;
74            this.xEnd = xEnd;
75            this.zStart = zStart;
76            this.zEnd = zEnd;
77
78            Node startNode = new Node(xStart, zStart, null, 0, EstimateCost(xStart, zStart), true, false);
79            open.Add(startNode);
80            nodeMap[xStart + zStart * level.Width] = startNode;
81
82            isPathFinding = true;
83            canPathFind = true;
84
85            if ((xStart == xEnd && zStart == zEnd))
86                isPathFinding = false;
87
88            if (xEnd < 0 || zEnd < 0 || xEnd >= level.Width || zEnd >= level.Height)
89            {
90#if DEBUG
91                Console.WriteLine("[FAIL] End is out of level boundaries.");
92#endif
93                canPathFind = false;
94            }
95            else if (tiles[xEnd + zEnd * level.Width])
96            {
97#if DEBUG
98                Console.WriteLine("[FAIL] End is solid.");
99#endif
100                canPathFind = false;
101            }
102        }
103
104        public void ApplySolidnessToPos(List<int> posList)
105        {
106            foreach (int i in posList)
107                tiles[i] = false;
108        }
109
110        public void FindPath(int maxVisits)
111        {
112#if DEBUG
113            DateTime startTime = DateTime.Now;
114#endif
115            calls++;
116
117            Node current = null;
118            int visits = 0;
119            do
120            {
121                visits++;
122                if (visits == maxVisits)
123                {
124#if DEBUG
125                    totalTime += DateTime.Now - startTime;
126                    Console.WriteLine("[PAUSE] Calls so far: " + calls + ". Time spent: " + ((DateTime.Now - startTime).TotalMilliseconds) + " ms.");
127#endif
128                    return;
129                }
130
131                current = GetBestOpenNode();
132                if (current == null)
133                {
134#if DEBUG
135                    totalTime += DateTime.Now - startTime;
136                    Console.WriteLine("[FAIL] No more open nodes! Calls: " + calls + ". Total time: " + totalTime.TotalMilliseconds + " ms.");
137#endif
138                    canPathFind = false;
139                    return;
140                }
141                current.IsClosed = true;
142                current.IsOpen = false;
143                open.Remove(current);
144
145                for (int x = current.X - 1; x <= current.X + 1; x++)
146                {
147                    for (int z = current.Z - 1; z <= current.Z + 1; z++)
148                    {
149                        if (x == current.X && z == current.Z) continue;
150                        if (x < 0 || z < 0 || x >= level.Width || z >= level.Height) continue;
151                        if (tiles[x + z * level.Width]) continue;
152                        if (tiles[x + current.Z * level.Width] && tiles[current.X + z * level.Width]) continue;
153
154                        int moveCost = (x == current.X || z == current.Z) ? MOVE_STRAIGHT : MOVE_VERTICAL;
155                        int travelCost = current.TravelCost + moveCost;
156
157                        Node neighbour = nodeMap[x + z * level.Width];
158
159                        if (neighbour != null)
160                        {
161                            if (neighbour.IsOpen && travelCost < neighbour.TravelCost)
162                            {
163                                neighbour.IsOpen = false;
164                                open.Remove(neighbour);
165                            }
166                            if (neighbour.IsClosed && travelCost < neighbour.TravelCost)
167                                neighbour.IsClosed = false;
168                        }
169                        if (neighbour == null || (!neighbour.IsClosed && !neighbour.IsOpen))
170                        {
171                            neighbour = new Node(x, z, current, travelCost, EstimateCost(x, z), true, false);
172                            open.Add(neighbour);
173                            nodeMap[x + z * level.Width] = neighbour;
174                        }
175                    }
176                }
177            } while (current.X != xEnd || current.Z != zEnd);
178
179            isPathFinding = false;
180
181#if DEBUG
182            totalTime += DateTime.Now - startTime;
183            Console.WriteLine("[FINISH] Calls: " + calls + ". Total time: " + totalTime.TotalMilliseconds + " ms.");
184#endif
185
186            path.Add(new Node(xEnd, zEnd, current, 0, 0, false, false));
187            Node reverse = current.Parent;
188            while (reverse.X != xStart || reverse.Z != zStart)
189            {
190                path.Add(reverse);
191                reverse = reverse.Parent;
192            }
193
194            path.Reverse();
195        }
196
197        private Node GetBestOpenNode()
198        {
199            Node result = null;
200
201            if (open.Count == 1)
202            {
203                result = open[0];
204                return result;
205            }
206
207            foreach (Node node in open)
208            {
209                if (result == null || node.TotalCost < result.TotalCost)
210                    result = node;
211            }
212
213            return result;
214        }
215    }
216}
Note: See TracBrowser for help on using the repository browser.