1 /* This file is part of the Spring engine (GPL v2 or later), see LICENSE.html */
2
3 #include "IPathFinder.h"
4 #include "PathFinderDef.h"
5 #include "PathLog.h"
6 #include "Sim/MoveTypes/MoveDefHandler.h"
7 #include "Sim/Objects/SolidObject.h"
8 #include "System/Log/ILog.h"
9
10
11 // these give the changes in (x, z) coors
12 // when moving one step in given direction
13 //
14 // NOTE: the choices of +1 for LEFT and UP are *not* arbitrary
15 // (they are related to GetBlockVertexOffset) and also need to
16 // be consistent with the PATHOPT_* flags (for PathDir2PathOpt)
17 int2 IPathFinder::PE_DIRECTION_VECTORS[PATH_DIRECTIONS] = {
18 int2(+1, 0), // PATHDIR_LEFT
19 int2(+1, +1), // PATHDIR_LEFT_UP
20 int2( 0, +1), // PATHDIR_UP
21 int2(-1, +1), // PATHDIR_RIGHT_UP
22 int2(-1, 0), // PATHDIR_RIGHT
23 int2(-1, -1), // PATHDIR_RIGHT_DOWN
24 int2( 0, -1), // PATHDIR_DOWN
25 int2(+1, -1), // PATHDIR_LEFT_DOWN
26 };
27
28 //FIXME why not use PATHDIR_* consts and merge code with top one
29 int2 IPathFinder::PF_DIRECTION_VECTORS_2D[PATH_DIRECTIONS << 1] = {
30 int2(0, 0),
31 int2(+1 * PATH_NODE_SPACING, 0 * PATH_NODE_SPACING), // PATHOPT_LEFT
32 int2(-1 * PATH_NODE_SPACING, 0 * PATH_NODE_SPACING), // PATHOPT_RIGHT
33 int2(0, 0), // PATHOPT_LEFT | PATHOPT_RIGHT
34 int2( 0 * PATH_NODE_SPACING, +1 * PATH_NODE_SPACING), // PATHOPT_UP
35 int2(+1 * PATH_NODE_SPACING, +1 * PATH_NODE_SPACING), // PATHOPT_LEFT | PATHOPT_UP
36 int2(-1 * PATH_NODE_SPACING, +1 * PATH_NODE_SPACING), // PATHOPT_RIGHT | PATHOPT_UP
37 int2(0, 0), // PATHOPT_LEFT | PATHOPT_RIGHT | PATHOPT_UP
38 int2( 0 * PATH_NODE_SPACING, -1 * PATH_NODE_SPACING), // PATHOPT_DOWN
39 int2(+1 * PATH_NODE_SPACING, -1 * PATH_NODE_SPACING), // PATHOPT_LEFT | PATHOPT_DOWN
40 int2(-1 * PATH_NODE_SPACING, -1 * PATH_NODE_SPACING), // PATHOPT_RIGHT | PATHOPT_DOWN
41 int2(0, 0),
42 int2(0, 0),
43 int2(0, 0),
44 int2(0, 0),
45 int2(0, 0),
46 };
47
48
49
IPathFinder(unsigned int _BLOCK_SIZE)50 IPathFinder::IPathFinder(unsigned int _BLOCK_SIZE)
51 : BLOCK_SIZE(_BLOCK_SIZE)
52 , BLOCK_PIXEL_SIZE(BLOCK_SIZE * SQUARE_SIZE)
53 , isEstimator(BLOCK_SIZE != 1)
54 , mStartBlockIdx(0)
55 , mGoalBlockIdx(0)
56 , mGoalHeuristic(0.0f)
57 , maxBlocksToBeSearched(0)
58 , testedBlocks(0)
59 , nbrOfBlocks(gs->mapx / BLOCK_SIZE, gs->mapy / BLOCK_SIZE)
60 , blockStates(nbrOfBlocks, int2(gs->mapx, gs->mapy))
61 {
62 }
63
64
~IPathFinder()65 IPathFinder::~IPathFinder()
66 {
67 //ResetSearch();
68 }
69
70
ResetSearch()71 void IPathFinder::ResetSearch()
72 {
73 openBlocks.Clear();
74
75 while (!dirtyBlocks.empty()) {
76 blockStates.ClearSquare(dirtyBlocks.back());
77 dirtyBlocks.pop_back();
78 }
79
80 testedBlocks = 0;
81 }
82
83
GetPath(const MoveDef & moveDef,const CPathFinderDef & pfDef,const CSolidObject * owner,float3 startPos,IPath::Path & path,const unsigned int maxNodes)84 IPath::SearchResult IPathFinder::GetPath(
85 const MoveDef& moveDef,
86 const CPathFinderDef& pfDef,
87 const CSolidObject* owner,
88 float3 startPos,
89 IPath::Path& path,
90 const unsigned int maxNodes
91 ) {
92 startPos.ClampInBounds();
93
94 // Clear the path
95 path.path.clear();
96 path.squares.clear();
97 path.pathCost = PATHCOST_INFINITY;
98
99 // initial calculations
100 if (isEstimator) {
101 maxBlocksToBeSearched = std::min(MAX_SEARCHED_NODES_PE - 8U, maxNodes);
102 } else {
103 maxBlocksToBeSearched = std::min(MAX_SEARCHED_NODES_PF - 8U, maxNodes);
104 }
105 mStartBlock.x = startPos.x / BLOCK_PIXEL_SIZE;
106 mStartBlock.y = startPos.z / BLOCK_PIXEL_SIZE;
107 mStartBlockIdx = BlockPosToIdx(mStartBlock);
108 assert((unsigned)mStartBlock.x < nbrOfBlocks.x && (unsigned)mStartBlock.y < nbrOfBlocks.y);
109
110 // Check cache (when there is one)
111 int2 goalBlock;
112 goalBlock.x = pfDef.goalSquareX / BLOCK_SIZE;
113 goalBlock.y = pfDef.goalSquareZ / BLOCK_SIZE;
114 const CPathCache::CacheItem* ci = GetCache(mStartBlock, goalBlock, pfDef.sqGoalRadius, moveDef.pathType, pfDef.synced);
115 if (ci != nullptr) {
116 path = ci->path;
117 return ci->result;
118 }
119
120 // Start up a new search
121 IPath::SearchResult result = InitSearch(moveDef, pfDef, owner);
122
123 // If search was successful, generate new path
124 if (result == IPath::Ok || result == IPath::GoalOutOfRange) {
125 FinishSearch(moveDef, pfDef, path);
126
127 // Save to cache
128 if (result == IPath::Ok) {
129 // add succesful paths to the cache
130 AddCache(&path, result, mStartBlock, goalBlock, pfDef.sqGoalRadius, moveDef.pathType, pfDef.synced);
131 }
132
133 if (LOG_IS_ENABLED(L_DEBUG)) {
134 LOG_L(L_DEBUG, "%s: Search completed.", (isEstimator) ? "PE" : "PF");
135 LOG_L(L_DEBUG, "Tested blocks: %u", testedBlocks);
136 LOG_L(L_DEBUG, "Open blocks: %u", openBlockBuffer.GetSize());
137 LOG_L(L_DEBUG, "Path length: " _STPF_, path.path.size());
138 LOG_L(L_DEBUG, "Path cost: %f", path.pathCost);
139 }
140 } else {
141 if (LOG_IS_ENABLED(L_DEBUG)) {
142 LOG_L(L_DEBUG, "%s: Search failed!", (isEstimator) ? "PE" : "PF");
143 LOG_L(L_DEBUG, "Tested blocks: %u", testedBlocks);
144 LOG_L(L_DEBUG, "Open blocks: %u", openBlockBuffer.GetSize());
145 }
146 }
147
148 return result;
149 }
150
151
152 // set up the starting point of the search
InitSearch(const MoveDef & moveDef,const CPathFinderDef & pfDef,const CSolidObject * owner)153 IPath::SearchResult IPathFinder::InitSearch(const MoveDef& moveDef, const CPathFinderDef& pfDef, const CSolidObject* owner)
154 {
155 int2 square = mStartBlock;
156 if (isEstimator) {
157 square = blockStates.peNodeOffsets[moveDef.pathType][mStartBlockIdx];
158 }
159 const bool isStartGoal = pfDef.IsGoal(square.x, square.y);
160
161 // although our starting square may be inside the goal radius, the starting coordinate may be outside.
162 // in this case we do not want to return CantGetCloser, but instead a path to our starting square.
163 if (isStartGoal && pfDef.startInGoalRadius)
164 return IPath::CantGetCloser;
165
166 // no, clean the system from last search
167 ResetSearch();
168
169 // mark and store the start-block
170 blockStates.nodeMask[mStartBlockIdx] &= PATHOPT_OBSOLETE; // clear all except PATHOPT_OBSOLETE
171 blockStates.nodeMask[mStartBlockIdx] |= PATHOPT_OPEN;
172 blockStates.fCost[mStartBlockIdx] = 0.0f;
173 blockStates.gCost[mStartBlockIdx] = 0.0f;
174 blockStates.SetMaxCost(NODE_COST_F, 0.0f);
175 blockStates.SetMaxCost(NODE_COST_G, 0.0f);
176
177 dirtyBlocks.push_back(mStartBlockIdx);
178
179 // start a new search and
180 // add the starting block to the open-blocks-queue
181 openBlockBuffer.SetSize(0);
182 PathNode* ob = openBlockBuffer.GetNode(openBlockBuffer.GetSize());
183 ob->fCost = 0.0f;
184 ob->gCost = 0.0f;
185 ob->nodePos = mStartBlock;
186 ob->nodeNum = mStartBlockIdx;
187 openBlocks.push(ob);
188
189 // mark starting point as best found position
190 mGoalBlockIdx = mStartBlockIdx;
191 mGoalHeuristic = pfDef.Heuristic(square.x, square.y);
192
193 // perform the search
194 IPath::SearchResult result = DoSearch(moveDef, pfDef, owner);
195
196 // if no improvements are found, then return CantGetCloser instead
197 if ((mGoalBlockIdx == mStartBlockIdx) && (!isStartGoal || pfDef.startInGoalRadius)) {
198 return IPath::CantGetCloser;
199 }
200
201 return result;
202 }
203