Debian Codegraph @

engine/pathfinder/astar.cpp

raw
1 // ____ _ __
2 // / __ )____ _____ | | / /___ ___________
3 // / __ / __ \/ ___/ | | /| / / __ `/ ___/ ___/
4 // / /_/ / /_/ (__ ) | |/ |/ / /_/ / / (__ )
5 // /_____/\____/____/ |__/|__/\__,_/_/ /____/
6 //
7 // A futuristic real-time strategy game.
8 // This file is part of Bos Wars.
9 //
10 /**@name astar.cpp - The a* path finder routines. */
11 //
12 // (c) Copyright 1999-2009 by Lutz Sammer, Fabrice Rossi, Russell Smith,
13 // Francois Beerten, Jimmy Salmon.
14 //
15 // This program is free software; you can redistribute it and/or modify
16 // it under the terms of the GNU General Public License as published by
17 // the Free Software Foundation; only version 2 of the License.
18 //
19 // This program is distributed in the hope that it will be useful,
20 // but WITHOUT ANY WARRANTY; without even the implied warranty of
21 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
22 // GNU General Public License for more details.
23 //
24 // You should have received a copy of the GNU General Public License
25 // along with this program; if not, write to the Free Software
26 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA
27 // 02111-1307, USA.
28 //
29
30 //@{
31
32 /*----------------------------------------------------------------------------
33 -- Includes
34 ----------------------------------------------------------------------------*/
35
36 #include "stratagus.h"
37
38 #include <stdlib.h>
39 #include <stdio.h>
40
41 #include "pathfinder.h"
42
43 /*----------------------------------------------------------------------------
44 -- Declarations
45 ----------------------------------------------------------------------------*/
46
47 struct Node {
48 char Direction; /// Direction for trace back
49 char InGoal; /// is this point in the goal
50 int CostFromStart; /// Real costs to reach this point
51 int CostToGoal; /// Estimated cost to goal
52 };
53
54 struct Open {
55 int X; /// X coordinate
56 int Y; /// Y coordinate
57 int O; /// Offset into matrix
58 int Costs; /// complete costs to goal
59 };
60
61 /// heuristic cost function for a*
62 #define AStarCosts(sx,sy,ex,ey) std::max(abs(sx - ex), abs(sy - ey))
63
64 /*----------------------------------------------------------------------------
65 -- Variables
66 ----------------------------------------------------------------------------*/
67
68 // Convert heading into direction.
69 // // N NE E SE S SW W NW
70 const int Heading2X[9] = { 0,+1,+1,+1, 0,-1,-1,-1, 0 };
71 const int Heading2Y[9] = { -1,-1, 0,+1,+1,+1, 0,-1, 0 };
72 const int XY2Heading[3][3] = { {7,6,5},{0,0,4},{1,2,3}};
73
74 /// cost matrix
75 static Node *AStarMatrix;
76
77 /// a list of close nodes, helps to speed up the matrix cleaning
78 static int *CloseSet;
79 static int CloseSetSize;
80 static int Threshold;
81 static int OpenSetMaxSize;
82 static int AStarMatrixSize;
83 #define MAX_CLOSE_SET_RATIO 4
84 #define MAX_OPEN_SET_RATIO 8 // 10,16 to small
85
86 /// see pathfinder.h
87 int AStarFixedUnitCrossingCost;// = MaxMapWidth * MaxMapHeight;
88 int AStarMovingUnitCrossingCost = 5;
89 bool AStarKnowUnseenTerrain = false;
90 int AStarUnknownTerrainCost = 2;
91
92 static int AStarMapWidth;
93 static int AStarMapHeight;
94
95 static int AStarGoalX;
96 static int AStarGoalY;
97
98 /**
99 ** The Open set is handled by a stored array
100 ** the end of the array holds the item with the smallest cost.
101 */
102
103 /// The set of Open nodes
104 static Open *OpenSet;
105 /// The size of the open node set
106 static int OpenSetSize;
107
108 static unsigned char *VisionTable[3];
109 static int *VisionLookup;
110
111 static int (STDCALL *CostMoveToCallback)(int x, int y, void *data);
112 static int *CostMoveToCache;
113 static const int CacheNotSet = -5;
114
115 /*----------------------------------------------------------------------------
116 -- Profile
117 ----------------------------------------------------------------------------*/
118
119 #ifdef ASTAR_PROFILE
120
121 #include <map>
122 #include <windows.h>
123 #undef max
124 #undef min
125 static std::map<std::string, LARGE_INTEGER> functionTimerMap;
126 struct ProfileData {
127 unsigned long Calls;
128 unsigned long TotalTime;
129 };
130 static std::map<std::string, ProfileData> functionProfiles;
131
132 inline void ProfileInit()
133 {
134 functionTimerMap.clear();
135 functionProfiles.clear();
136 }
137
138 inline void ProfileBegin(const std::string &function)
139 {
140 LARGE_INTEGER counter;
141 if (!QueryPerformanceCounter(&counter)) {
142 return;
143 }
144 functionTimerMap[function] = counter;
145 }
146
147 inline void ProfileEnd(const std::string &function)
148 {
149 LARGE_INTEGER counter;
150 if (!QueryPerformanceCounter(&counter)) {
151 return;
152 }
153 unsigned long time = (unsigned long)(counter.QuadPart - functionTimerMap[function].QuadPart);
154 ProfileData *data = &functionProfiles[function];
155 data->Calls++;
156 data->TotalTime += time;
157 }
158
159 inline void ProfilePrint()
160 {
161 LARGE_INTEGER frequency;
162 if (!QueryPerformanceFrequency(&frequency)) {
163 return;
164 }
165
166 std::map<std::string, ProfileData>::iterator i;
167
168 FILE *fd = fopen("profile.txt", "wb");
169 fprintf(fd, " total\t calls\t per\tname\n");
170
171 for (i = functionProfiles.begin(); i != functionProfiles.end(); ++i) {
172 ProfileData *data = &i->second;
173 fprintf(fd, "%9.3f\t%9lu\t%9.3f\t%s\n",
174 (double)data->TotalTime / frequency.QuadPart * 1000.0,
175 data->Calls,
176 (double)data->TotalTime / frequency.QuadPart * 1000.0 / data->Calls,
177 i->first.c_str());
178 }
179
180 fclose(fd);
181 }
182
183 #else
184 #define ProfileInit()
185 #define ProfileBegin(f)
186 #define ProfileEnd(f)
187 #define ProfilePrint()
188 #endif
189
190 /*----------------------------------------------------------------------------
191 -- Functions
192 ----------------------------------------------------------------------------*/
193
194 // FIXME: this is duplicated from map_fog.cpp
195 static void InitVisionTable(void)
196 {
197 ProfileBegin("InitVisionTable");
198
199 int *visionlist;
200 int maxsize;
201 int sizex;
202 int sizey;
203 int maxsearchsize;
204 int i;
205 int VisionTablePosition;
206 int marker;
207 int direction;
208 int right;
209 int up;
210 int repeat;
211
212 // Initialize Visiontable to large size, can't be more entries than tiles.
213 VisionTable[0] = new unsigned char[AStarMapWidth * AStarMapWidth];
214 VisionTable[1] = new unsigned char[AStarMapWidth * AStarMapWidth];
215 VisionTable[2] = new unsigned char[AStarMapWidth * AStarMapWidth];
216
217 VisionLookup = new int[AStarMapWidth + 2];
218
219 visionlist = new int[AStarMapWidth * AStarMapWidth];
220 //*2 as diagonal distance is longer
221
222 maxsize = AStarMapWidth;
223 maxsearchsize = AStarMapWidth;
224 // Fill in table of map size
225 for (sizex = 0; sizex < maxsize; ++sizex) {
226 for (sizey = 0; sizey < maxsize; ++sizey) {
227 visionlist[sizey * maxsize + sizex] = isqrt(sizex * sizex + sizey * sizey);
228 }
229 }
230
231 VisionLookup[0] = 0;
232 i = 1;
233 VisionTablePosition = 0;
234 while (i < maxsearchsize) {
235 // Set Lookup Table
236 VisionLookup[i] = VisionTablePosition;
237 // Put in Null Marker
238 VisionTable[0][VisionTablePosition] = i;
239 VisionTable[1][VisionTablePosition] = 0;
240 VisionTable[2][VisionTablePosition] = 0;
241 ++VisionTablePosition;
242
243
244 // find i in left column
245 marker = maxsize * i;
246 direction = 0;
247 right = 0;
248 up = 0;
249
250 // If not on top row, continue
251 do {
252 repeat = 0;
253 do {
254 // search for repeating
255 // Test Right
256 if ((repeat == 0 || direction == 1) && visionlist[marker + 1] == i) {
257 right = 1;
258 up = 0;
259 ++repeat;
260 direction = 1;
261 ++marker;
262 } else if ((repeat == 0 || direction == 2) && visionlist[marker - maxsize] == i) {
263 up = 1;
264 right = 0;
265 ++repeat;
266 direction = 2;
267 marker = marker - maxsize;
268 } else if ((repeat == 0 || direction == 3) && visionlist[marker + 1 - maxsize] == i &&
269 visionlist[marker - maxsize] != i && visionlist[marker + 1] != i) {
270 up = 1;
271 right = 1;
272 ++repeat;
273 direction = 3;
274 marker = marker + 1 - maxsize;
275 } else {
276 direction = 0;
277 break;
278 }
279
280 // search right
281 // search up - store as down.
282 // search diagonal
283 } while (direction && marker > (maxsize * 2));
284 if (right || up) {
285 VisionTable[0][VisionTablePosition] = repeat;
286 VisionTable[1][VisionTablePosition] = right;
287 VisionTable[2][VisionTablePosition] = up;
288 ++VisionTablePosition;
289 }
290 } while (marker > (maxsize * 2));
291 ++i;
292 }
293
294 delete[] visionlist;
295
296 ProfileEnd("InitVisionTable");
297 }
298
299 /**
300 ** Init A* data structures
301 */
302 void InitAStar(int mapWidth, int mapHeight, int (STDCALL *costMoveTo)(int x, int y, void *data))
303 {
304 // Should only be called once
305 Assert(!AStarMatrix);
306
307 AStarMapWidth = mapWidth;
308 AStarMapHeight = mapHeight;
309 CostMoveToCallback = costMoveTo;
310
311 AStarMatrixSize = sizeof(Node) * AStarMapWidth * AStarMapHeight;
312 AStarMatrix = new Node[AStarMapWidth * AStarMapHeight];
313 memset(AStarMatrix, 0, AStarMatrixSize);
314
315 Threshold = AStarMapWidth * AStarMapHeight / MAX_CLOSE_SET_RATIO;
316 CloseSet = new int[Threshold];
317
318 OpenSetMaxSize = AStarMapWidth * AStarMapHeight / MAX_OPEN_SET_RATIO;
319 OpenSet = new Open[OpenSetMaxSize];
320
321 CostMoveToCache = new int[AStarMapWidth * AStarMapHeight];
322
323 InitVisionTable();
324 ProfileInit();
325 }
326
327 /**
328 ** Free A* data structure
329 */
330 void FreeAStar(void)
331 {
332 delete[] AStarMatrix;
333 AStarMatrix = NULL;
334 delete[] CloseSet;
335 CloseSet = NULL;
336 CloseSetSize = 0;
337 delete[] OpenSet;
338 OpenSet = NULL;
339 OpenSetSize = 0;
340 delete[] CostMoveToCache;
341 CostMoveToCache = NULL;
342
343 delete[] VisionLookup;
344 VisionLookup = NULL;
345 delete[] VisionTable[0];
346 VisionTable[0] = NULL;
347 delete[] VisionTable[1];
348 VisionTable[1] = NULL;
349 delete[] VisionTable[2];
350 VisionTable[2] = NULL;
351
352 ProfilePrint();
353 }
354
355 /**
356 ** Prepare pathfinder.
357 */
358 static void AStarPrepare(void)
359 {
360 memset(AStarMatrix, 0, AStarMatrixSize);
361 }
362
363 /**
364 ** Clean up A*
365 */
366 static void AStarCleanUp()
367 {
368 ProfileBegin("AStarCleanUp");
369
370 if (CloseSetSize >= Threshold) {
371 AStarPrepare();
372 } else {
373 for (int i = 0; i < CloseSetSize; ++i) {
374 AStarMatrix[CloseSet[i]].CostFromStart = 0;
375 AStarMatrix[CloseSet[i]].InGoal = 0;
376 }
377 }
378
379 ProfileEnd("AStarCleanUp");
380 }
381
382 /**
383 ** Find the best node in the current open node set
384 ** Returns the position of this node in the open node set
385 */
386 #define AStarFindMinimum() (OpenSetSize - 1)
387
388
389 /**
390 ** Remove the minimum from the open node set
391 */
392 static void AStarRemoveMinimum(int pos)
393 {
394 Assert(pos == OpenSetSize - 1);
395
396 OpenSetSize--;
397 }
398
399 /**
400 ** Add a new node to the open set (and update the heap structure)
401 **
402 ** @return 0 or PF_FAILED
403 */
404 static int AStarAddNode(int x, int y, int o, int costs)
405 {
406 ProfileBegin("AStarAddNode");
407
408 int bigi, smalli;
409 int midcost;
410 int midi;
411 int costToGoal;
412 int midCostToGoal;
413 int dist;
414 int midDist;
415
416 if (OpenSetSize + 1 >= OpenSetMaxSize) {
417 fprintf(stderr, "A* internal error: raise Open Set Max Size "
418 "(current value %d)\n", OpenSetMaxSize);
419 ProfileEnd("AStarAddNode");
420 return PF_FAILED;
421 }
422
423 costToGoal = AStarMatrix[o].CostToGoal;
424 dist = abs(x - AStarGoalX) + abs(y - AStarGoalY);
425
426 // find where we should insert this node.
427 bigi = 0;
428 smalli = OpenSetSize;
429
430 // binary search where to insert the new node
431 while (bigi < smalli) {
432 midi = (smalli + bigi) >> 1;
433 midcost = OpenSet[midi].Costs;
434 midCostToGoal = AStarMatrix[OpenSet[midi].O].CostToGoal;
435 midDist = abs(OpenSet[midi].X - AStarGoalX) + abs(OpenSet[midi].Y - AStarGoalY);
436 if (costs > midcost || (costs == midcost &&
437 (costToGoal > midCostToGoal || (costToGoal == midCostToGoal &&
438 dist > midDist)))) {
439 smalli = midi;
440 } else if (costs < midcost || (costs == midcost &&
441 (costToGoal < midCostToGoal || (costToGoal == midCostToGoal &&
442 dist < midDist)))) {
443 if (bigi == midi) {
444 bigi++;
445 } else {
446 bigi = midi;
447 }
448 } else {
449 bigi = midi;
450 smalli = midi;
451 }
452 }
453
454 if (OpenSetSize > bigi) {
455 // free a the slot for our node
456 memmove(&OpenSet[bigi+1], &OpenSet[bigi], (OpenSetSize - bigi) * sizeof(Open));
457 }
458
459 // fill our new node
460 OpenSet[bigi].X = x;
461 OpenSet[bigi].Y = y;
462 OpenSet[bigi].O = o;
463 OpenSet[bigi].Costs = costs;
464 ++OpenSetSize;
465
466 ProfileEnd("AStarAddNode");
467
468 return 0;
469 }
470
471 /**
472 ** Change the cost associated to an open node.
473 ** Can be further optimised knowing that the new cost MUST BE LOWER
474 ** than the old one.
475 */
476 static void AStarReplaceNode(int pos, int costs)
477 {
478 ProfileBegin("AStarReplaceNode");
479
480 Open node;
481
482 // Remove the outdated node
483 node = OpenSet[pos];
484 OpenSetSize--;
485 memmove(&OpenSet[pos], &OpenSet[pos+1], sizeof(Open) * (OpenSetSize-pos));
486
487 // Re-add the node with the new cost
488 AStarAddNode(node.X, node.Y, node.O, node.Costs);
489
490 ProfileEnd("AStarReplaceNode");
491 }
492
493
494 /**
495 ** Check if a node is already in the open set.
496 **
497 ** @return -1 if not found and the position of the node in the table if found.
498 */
499 static int AStarFindNode(int eo)
500 {
501 ProfileBegin("AStarFindNode");
502
503 for (int i = 0; i < OpenSetSize; ++i) {
504 if (OpenSet[i].O == eo) {
505 ProfileEnd("AStarFindNode");
506 return i;
507 }
508 }
509 ProfileEnd("AStarFindNode");
510 return -1;
511 }
512
513 /**
514 ** Add a node to the closed set
515 */
516 static void AStarAddToClose(int node)
517 {
518 if (CloseSetSize < Threshold) {
519 CloseSet[CloseSetSize++] = node;
520 }
521 }
522
523 /**
524 ** Compute the cost of crossing tile (x,y)
525 **
526 ** @param x X tile where to move.
527 ** @param y Y tile where to move.
528 ** @param data user data.
529 **
530 ** @return -1 -> impossible to cross.
531 ** 0 -> no induced cost, except move
532 ** >0 -> costly tile
533 */
534 static int CostMoveTo(int x, int y, void *data)
535 {
536 int *c = &CostMoveToCache[y * AStarMapWidth + x];
537 if (*c != CacheNotSet) {
538 return *c;
539 }
540 return (*c = CostMoveToCallback(x, y, data));
541 }
542
543 /**
544 ** MarkAStarGoal
545 */
546 static int AStarMarkGoal(int gx, int gy, int gw, int gh,
547 int tilesizex, int tilesizey, int minrange, int maxrange, void *data)
548 {
549 ProfileBegin("AStarMarkGoal");
550
551 int cx[4];
552 int cy[4];
553 int steps;
554 int cycle;
555 int x;
556 int y;
557 bool goal_reachable;
558 int quad;
559 int eo;
560 int filler;
561 int range;
562 int z1, z2;
563 bool doz1, doz2;
564
565 goal_reachable = false;
566
567 if (minrange == 0 && maxrange == 0 && gw == 0 && gh == 0) {
568 if (gx + tilesizex >= AStarMapWidth ||
569 gy + tilesizey >= AStarMapHeight) {
570 ProfileEnd("AStarMarkGoal");
571 return 0;
572 }
573 if (CostMoveTo(gx, gy, data) >= 0) {
574 AStarMatrix[gx + gy * AStarMapWidth].InGoal = 1;
575 ProfileEnd("AStarMarkGoal");
576 return 1;
577 } else {
578 ProfileEnd("AStarMarkGoal");
579 return 0;
580 }
581 }
582
583 if (minrange == 0) {
584 int sx = std::max(gx, 0);
585 int ex = std::min(gx + gw, AStarMapWidth - tilesizex);
586 for (x = sx; x < ex; ++x) {
587 int sy = std::max(gy, 0);
588 int ey = std::min(gy + gh, AStarMapHeight - tilesizey);
589 for (y = sy; y < ey; ++y) {
590 if (CostMoveTo(x, y, data) >= 0) {
591 AStarMatrix[y * AStarMapWidth + x].InGoal = 1;
592 goal_reachable = true;
593 }
594 AStarAddToClose(y * AStarMapWidth + x);
595 }
596 }
597 }
598
599 if (gw) {
600 gw--;
601 }
602 if (gh) {
603 gh--;
604 }
605
606 int sx = std::max(gx, 0);
607 int ex = std::min(gx + gw, AStarMapWidth - tilesizex);
608 int sy = std::max(gy, 0);
609 int ey = std::min(gy + gh, AStarMapHeight - tilesizey);
610
611 // Mark top, bottom, left, right
612 for (range = minrange; range <= maxrange; ++range) {
613 z1 = gy - range;
614 z2 = gy + range + gh;
615 doz1 = z1 >= 0 && z1 + tilesizex - 1 < AStarMapHeight;
616 doz2 = z2 >= 0 && z2 + tilesizey - 1 < AStarMapHeight;
617 if (doz1 || doz2) {
618 // Mark top and bottom of goal
619 for (x = sx; x <= ex; ++x) {
620 if (doz1 && CostMoveTo(x, z1, data) >= 0) {
621 AStarMatrix[z1 * AStarMapWidth + x].InGoal = 1;
622 AStarAddToClose(z1 * AStarMapWidth + x);
623 goal_reachable = true;
624 }
625 if (doz2 && CostMoveTo(x, z2, data) >= 0) {
626 AStarMatrix[z2 * AStarMapWidth + x].InGoal = 1;
627 AStarAddToClose(z2 * AStarMapWidth + x);
628 goal_reachable = true;
629 }
630 }
631 }
632 z1 = gx - range;
633 z2 = gx + gw + range;
634 doz1 = z1 >= 0 && z1 + tilesizex - 1 < AStarMapWidth;
635 doz2 = z2 >= 0 && z2 + tilesizex - 1 < AStarMapWidth;
636 if (doz1 || doz2) {
637 // Mark left and right of goal
638 for (y = sy; y <= ey; ++y) {
639 if (doz1 && CostMoveTo(z1, y, data) >= 0) {
640 AStarMatrix[y * AStarMapWidth + z1].InGoal = 1;
641 AStarAddToClose(y * AStarMapWidth + z1);
642 goal_reachable = true;
643 }
644 if (doz2 && CostMoveTo(z2, y, data) >= 0) {
645 AStarMatrix[y * AStarMapWidth + z2].InGoal = 1;
646 AStarAddToClose(y * AStarMapWidth + z2);
647 goal_reachable = true;
648 }
649 }
650 }
651 } // Go Through the Ranges
652
653 // Mark Goal Border in Matrix
654
655 // Mark Edges of goal
656
657 steps = VisionLookup[minrange];
658
659 while (VisionTable[0][steps] <= maxrange) {
660 // 0 - Top right Quadrant
661 cx[0] = gx + gw;
662 cy[0] = gy - VisionTable[0][steps];
663 // 1 - Top left Quadrant
664 cx[1] = gx;
665 cy[1] = gy - VisionTable[0][steps];
666 // 2 - Bottom Left Quadrant
667 cx[2] = gx;
668 cy[2] = gy + VisionTable[0][steps]+gh;
669 // 3 - Bottom Right Quadrant
670 cx[3] = gx + gw;
671 cy[3] = gy + VisionTable[0][steps]+gh;
672
673 ++steps; // Move past blank marker
674 while (VisionTable[1][steps] != 0 || VisionTable[2][steps] != 0) {
675 // Loop through for repeat cycle
676 cycle = 0;
677 while (cycle++ < VisionTable[0][steps]) {
678 // If we travelled on an angle, mark down as well.
679 if (VisionTable[1][steps] == VisionTable[2][steps]) {
680 // do down
681 for (quad = 0; quad < 4; ++quad) {
682 if (quad < 2) {
683 filler = 1;
684 } else {
685 filler = -1;
686 }
687 if (cx[quad] >= 0 && cx[quad] + tilesizex - 1 < AStarMapWidth &&
688 cy[quad] + filler >= 0 && cy[quad] + filler + tilesizey - 1 < AStarMapHeight &&
689 CostMoveTo(cx[quad], cy[quad] + filler, data) >= 0) {
690 eo = (cy[quad] + filler) * AStarMapWidth + cx[quad];
691 AStarMatrix[eo].InGoal = 1;
692 AStarAddToClose(eo);
693 goal_reachable = true;
694 }
695 }
696 }
697
698 cx[0] += VisionTable[1][steps];
699 cy[0] += VisionTable[2][steps];
700 cx[1] -= VisionTable[1][steps];
701 cy[1] += VisionTable[2][steps];
702 cx[2] -= VisionTable[1][steps];
703 cy[2] -= VisionTable[2][steps];
704 cx[3] += VisionTable[1][steps];
705 cy[3] -= VisionTable[2][steps];
706
707 // Mark Actually Goal curve change
708 for (quad = 0; quad < 4; ++quad) {
709 if (cx[quad] >= 0 && cx[quad] + tilesizex - 1 < AStarMapWidth &&
710 cy[quad] >= 0 && cy[quad] + tilesizey - 1 < AStarMapHeight &&
711 CostMoveTo(cx[quad], cy[quad], data) >= 0) {
712 eo = cy[quad] * AStarMapWidth + cx[quad];
713 AStarMatrix[eo].InGoal = 1;
714 AStarAddToClose(eo);
715 goal_reachable = true;
716 }
717 }
718 }
719 ++steps;
720 }
721 }
722
723 ProfileEnd("AStarMarkGoal");
724 return goal_reachable;
725 }
726
727 /**
728 ** Save the path
729 **
730 ** @return The length of the path
731 */
732 static int AStarSavePath(int startX, int startY, int endX, int endY, char *path, int pathLen)
733 {
734 ProfileBegin("AStarSavePath");
735
736 int currX, currY;
737 int fullPathLength;
738 int pathPos;
739 int direction;
740
741 // Figure out the full path length
742 fullPathLength = 0;
743 currX = endX;
744 currY = endY;
745 while (currX != startX || currY != startY) {
746 direction = AStarMatrix[currY * AStarMapWidth + currX].Direction;
747 currX -= Heading2X[direction];
748 currY -= Heading2Y[direction];
749 fullPathLength++;
750 }
751
752 // Save as much of the path as we can
753 if (path) {
754 pathLen = std::min(fullPathLength, pathLen);
755 pathPos = fullPathLength;
756 currX = endX;
757 currY = endY;
758 while ((currX != startX || currY != startY) && path != NULL) {
759 direction = AStarMatrix[currY * AStarMapWidth + currX].Direction;
760 currX -= Heading2X[direction];
761 currY -= Heading2Y[direction];
762 --pathPos;
763 if (pathPos < pathLen) {
764 path[pathLen - pathPos - 1] = direction;
765 }
766 }
767 }
768
769 ProfileEnd("AStarSavePath");
770 return fullPathLength;
771 }
772
773 /**
774 ** Optimization to find a simple path
775 ** Check if we're at the goal or if it's 1 tile away
776 */
777 static int AStarFindSimplePath(int sx, int sy, int gx, int gy, int gw, int gh,
778 int tilesizex, int tilesizey, int minrange, int maxrange, char *path, int pathlen, void *data)
779 {
780 ProfileBegin("AStarFindSimplePath");
781 // At exact destination point already
782 if (gx == sx && gy == sy && minrange == 0) {
783 ProfileEnd("AStarFindSimplePath");
784 return PF_REACHED;
785 }
786
787 // Don't allow unit inside destination area
788 if (gx <= sx && sx <= gx + gw - 1 &&
789 gy <= sy && sy <= gy + gh - 1) {
790 return PF_FAILED;
791 }
792
793 int dx = abs(gx - sx);
794 int dy = abs(gy - sy);
795 int distance = isqrt(dx * dx + dy * dy);
796
797 // Within range of destination
798 if (minrange <= distance && distance <= maxrange) {
799 ProfileEnd("AStarFindSimplePath");
800 return PF_REACHED;
801 }
802
803 if (dx <= 1 && dy <= 1) {
804 // Move to adjacent cell
805 if (CostMoveTo(gx, gy, data) == -1) {
806 ProfileEnd("AStarFindSimplePath");
807 return PF_UNREACHABLE;
808 }
809
810 if (path) {
811 path[0] = XY2Heading[gx - sx + 1][gy - sy + 1];
812 }
813 ProfileEnd("AStarFindSimplePath");
814 return 1;
815 }
816
817 ProfileEnd("AStarFindSimplePath");
818 return PF_FAILED;
819 }
820
821 /**
822 ** Find path.
823 */
824 int AStarFindPath(int sx, int sy, int gx, int gy, int gw, int gh,
825 int tilesizex, int tilesizey, int minrange, int maxrange, char *path, int pathlen, void *data)
826 {
827 ProfileBegin("AStarFindPath");
828
829 int i;
830 int j;
831 int o;
832 int ex;
833 int ey;
834 int eo;
835 int x;
836 int y;
837 int px;
838 int py;
839 int shortest;
840 // int counter;
841 int new_cost;
842 int costToGoal;
843 int path_length;
844 int ret = PF_FAILED;
845
846 AStarGoalX = gx;
847 AStarGoalY = gy;
848
849 //
850 // Check for simple cases first
851 //
852 i = AStarFindSimplePath(sx, sy, gx, gy, gw, gh, tilesizex, tilesizey,
853 minrange, maxrange, path, pathlen, data);
854 if (i != PF_FAILED) {
855 ret = i;
856 goto Cleanup;
857 }
858
859 //
860 // Initialize
861 //
862 AStarCleanUp();
863
864 for (i = 0; i < AStarMapWidth * AStarMapHeight; ++i) {
865 CostMoveToCache[i] = CacheNotSet;
866 }
867
868 OpenSetSize = 0;
869 CloseSetSize = 0;
870 x = sx;
871 y = sy;
872
873 if (!AStarMarkGoal(gx, gy, gw, gh, tilesizex, tilesizey,
874 minrange, maxrange, data)) {
875 // goal is not reachable
876 ret = PF_UNREACHABLE;
877 goto Cleanup;
878 }
879
880 eo = y * AStarMapWidth + x;
881 // it is quite important to start from 1 rather than 0, because we use
882 // 0 as a way to represent nodes that we have not visited yet.
883 AStarMatrix[eo].CostFromStart = 1;
884 // 8 to say we are came from nowhere.
885 AStarMatrix[eo].Direction = 8;
886
887 // place start point in open, it that failed, try another pathfinder
888 costToGoal = AStarCosts(x, y, gx, gy);
889 AStarMatrix[eo].CostToGoal = costToGoal;
890 if (AStarAddNode(x, y, eo, 1 + costToGoal) == PF_FAILED) {
891 ret = PF_FAILED;
892 goto Cleanup;
893 }
894 AStarAddToClose(OpenSet[0].O);
895 if (AStarMatrix[eo].InGoal) {
896 ret = PF_REACHED;
897 goto Cleanup;
898 }
899
900 // counter = AStarMapWidth * AStarMapHeight;
901
902 //
903 // Begin search
904 //
905 while (1) {
906 //
907 // Find the best node of from the open set
908 //
909 shortest = AStarFindMinimum();
910 x = OpenSet[shortest].X;
911 y = OpenSet[shortest].Y;
912 o = OpenSet[shortest].O;
913
914 AStarRemoveMinimum(shortest);
915
916 //
917 // If we have reached the goal, then exit.
918 if (AStarMatrix[o].InGoal == 1) {
919 ex = x;
920 ey = y;
921 break;
922 }
923
924 #if 0
925 //
926 // If we have looked too long, then exit.
927 //
928 if (!counter--) {
929 //
930 // FIXME: Select a "good" point from the open set.
931 // Nearest point to goal.
932 AstarDebugPrint("way too long\n");
933 ret = PF_FAILED;
934 goto Cleanup;
935 }
936 #endif
937
938 //
939 // Generate successors of this node.
940
941 // Node that this node was generated from.
942 px = x - Heading2X[(int)AStarMatrix[x + AStarMapWidth * y].Direction];
943 py = y - Heading2Y[(int)AStarMatrix[x + AStarMapWidth * y].Direction];
944
945 for (i = 0; i < 8; ++i) {
946 ex = x + Heading2X[i];
947 ey = y + Heading2Y[i];
948
949 // Don't check the tile we came from, it's not going to be better
950 // Should reduce load on A*
951
952 if (ex == px && ey == py) {
953 continue;
954 }
955 //
956 // Outside the map or can't be entered.
957 //
958 if (ex < 0 || ex + tilesizex - 1 >= AStarMapWidth ||
959 ey < 0 || ey + tilesizey - 1 >= AStarMapHeight) {
960 continue;
961 }
962
963 // if the point is "move to"-able and
964 // if we have not reached this point before,
965 // or if we have a better path to it, we add it to open set
966 new_cost = CostMoveTo(ex, ey, data);
967 if (new_cost == -1) {
968 // uncrossable tile
969 continue;
970 }
971
972 // Add a cost for walking to make paths more realistic for the user.
973 new_cost++;
974 eo = ey * AStarMapWidth + ex;
975 new_cost += AStarMatrix[o].CostFromStart;
976 if (AStarMatrix[eo].CostFromStart == 0) {
977 // we are sure the current node has not been already visited
978 AStarMatrix[eo].CostFromStart = new_cost;
979 AStarMatrix[eo].Direction = i;
980 costToGoal = AStarCosts(ex, ey, gx, gy);
981 AStarMatrix[eo].CostToGoal = costToGoal;
982 if (AStarAddNode(ex, ey, eo, AStarMatrix[eo].CostFromStart + costToGoal) == PF_FAILED) {
983 ret = PF_FAILED;
984 goto Cleanup;
985 }
986 // we add the point to the close set
987 AStarAddToClose(eo);
988 } else if (new_cost < AStarMatrix[eo].CostFromStart) {
989 // Already visited node, but we have here a better path
990 // I know, it's redundant (but simpler like this)
991 AStarMatrix[eo].CostFromStart = new_cost;
992 AStarMatrix[eo].Direction = i;
993 // this point might be already in the OpenSet
994 j = AStarFindNode(eo);
995 if (j == -1) {
996 costToGoal = AStarCosts(ex, ey, gx, gy);
997 AStarMatrix[eo].CostToGoal = costToGoal;
998 if (AStarAddNode(ex, ey, eo,
999 AStarMatrix[eo].CostFromStart + costToGoal) == PF_FAILED) {
1000 ret = PF_FAILED;
1001 goto Cleanup;
1002 }
1003 } else {
1004 costToGoal = AStarCosts(ex, ey, gx, gy);
1005 AStarMatrix[eo].CostToGoal = costToGoal;
1006 AStarReplaceNode(j, AStarMatrix[eo].CostFromStart + costToGoal);
1007 }
1008 // we don't have to add this point to the close set
1009 }
1010 }
1011 if (OpenSetSize <= 0) { // no new nodes generated
1012 ret = PF_UNREACHABLE;
1013 goto Cleanup;
1014 }
1015 }
1016
1017 path_length = AStarSavePath(sx, sy, ex, ey, path, pathlen);
1018
1019 ret = path_length;
1020
1021 Cleanup:
1022 ProfileEnd("AStarFindPath");
1023 return ret;
1024 }
1025
1026 struct StatsNode
1027 {
1028 StatsNode() : Direction(0), InGoal(0), CostFromStart(0), Costs(0) {}
1029
1030 int Direction;
1031 int InGoal;
1032 int CostFromStart;
1033 int Costs;
1034 int CostToGoal;
1035 };
1036
1037 StatsNode *AStarGetStats()
1038 {
1039 StatsNode *stats = new StatsNode[AStarMapWidth * AStarMapHeight];
1040 StatsNode *s = stats;
1041 Node *m = AStarMatrix;
1042
1043 for (int j = 0; j < AStarMapHeight; ++j) {
1044 for (int i = 0; i < AStarMapWidth; ++i) {
1045 s->Direction = m->Direction;
1046 s->InGoal = m->InGoal;
1047 s->CostFromStart = m->CostFromStart;
1048 s->CostToGoal = m->CostToGoal;
1049 ++s;
1050 ++m;
1051 }
1052 }
1053
1054 for (int i = 0; i < OpenSetSize; ++i) {
1055 stats[OpenSet[i].O].Costs = OpenSet[i].Costs;
1056 }
1057
1058 return stats;
1059 }
1060
1061 void AStarFreeStats(StatsNode *stats)
1062 {
1063 delete[] stats;
1064 }
1065
1066 /*----------------------------------------------------------------------------
1067 -- Configurable costs
1068 ----------------------------------------------------------------------------*/
1069
1070 // AStarFixedUnitCrossingCost
1071 void SetAStarFixedUnitCrossingCost(int cost) {
1072 if (cost <= 3) {
1073 fprintf(stderr, "AStarFixedUnitCrossingCost must be greater than 3\n");
1074 return;
1075 }
1076 AStarFixedUnitCrossingCost = cost;
1077 }
1078 int GetAStarFixedUnitCrossingCost() {
1079 return AStarFixedUnitCrossingCost;
1080 }
1081
1082 // AStarMovingUnitCrossingCost
1083 void SetAStarMovingUnitCrossingCost(int cost) {
1084 if (cost <= 3) {
1085 fprintf(stderr, "AStarMovingUnitCrossingCost must be greater than 3\n");
1086 return;
1087 }
1088 AStarMovingUnitCrossingCost = cost;
1089 }
1090 int GetAStarMovingUnitCrossingCost() {
1091 return AStarMovingUnitCrossingCost;
1092 }
1093
1094 // AStarUnknownTerrainCost
1095 void SetAStarUnknownTerrainCost(int cost) {
1096 if (cost < 0) {
1097 fprintf(stderr, "AStarUnknownTerrainCost must be non-negative\n");
1098 return;
1099 }
1100 AStarUnknownTerrainCost = cost;
1101 }
1102 int GetAStarUnknownTerrainCost() {
1103 return AStarUnknownTerrainCost;
1104 }
1105
1106 //@}