1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
|
/* $Id$ */
#include "../yapf_base.hpp"
struct CYapfMap1
{
enum {xMax = 32, yMax = 68};
static int MapZ(int x, int y)
{
static const char *z1[yMax] = {
"AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA",
"A000000000000000000000000000000000000000000000000000000000000000000A",
"A000000000000000000000000000000000000000000000000000000000000000000A",
"A000000000001000000000000000000000000000000000000000000000000000000A",
"A000000000001000000000000000000000000000000000000000000000000000000A",
"A000033333333333000000000000000000000000000000000000000000000000000A",
"A000030000000000000000000000000000000000000000000000000000000000000A",
"A000030000000000000000000000000000000000000000000000000000000000000A",
"A000030000000000000000000000000000000000000000000000000000000000000A",
"A000030000000000000000000000000000000000000000000000000000000000000A",
"A000030000000000000000000000000000000000000000000000000000000000000A",
"A210030000000000000000000000000000000000000000000000000000000000000A",
"A000000000000000000000000000000000000000000000000000000000000000000A",
"A000000000000000000000000000000000000000000000000000000000000000000A",
"A000000000000000000000000000000000000000000000000000000000000000000A",
"A000000000000000000000000000000000000000000000000000000000000000000A",
"A011333323333333233333333333333333333333333333333333333333333000000A",
"A000030000000000000000000000000000000000000000000000000000003000000A",
"A000030000000000000000000000000000000000000000000000000000003000000A",
"A000030000000000000000000000000000000000000000000000000000003000000A",
"A210030000000000000000000000000000000000000000000000000000003000000A",
"A000030000000000000000000000000000000000000000000000000000003000000A",
"A000030000000000000000000000000000000000000000000000000000003000000A",
"A000230000000000000000000000000000000000000000000000000000003000000A",
"A000030000000000000000000000000000000000000000000000000000003000000A",
"A000030000000000000000000000000000000000000000000000000000003000000A",
"A000030000000000000000000000000000000000000000000000000000003000000A",
"A000000000000000000000000000003333333333333333333333333333333000000A",
"A000000000000000000000000000000000000000000000000000000000000000000A",
"A000000000000000000000000000000000000000000000000000000000000000000A",
"A000000000000000000000000000000000000000000000000000000000000000000A",
"AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA",
};
static const char *z2[yMax] = {
"AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA",
"A000000000000000000000000000000000000000000000000000000000000000000A",
"A003333333333333333333333333333333333333333333333333300000000000000A",
"A003000000001000000000000000000000000000000000000000300000000000000A",
"A003000000001000000000000000000000000000000000000000300000000000000A",
"A003333333333333333333333333333333333333300000000000300000000000000A",
"A000030000000000000000000000000000000000300000000000300000000000000A",
"A000030000000000000000000000000000000000333333333333300000000000000A",
"A000030000000000000000000000000000000000300000000000000000000000000A",
"A000030000000000000000000000000000000000300000000000000000000000000A",
"A000030000000000000000000000000000000000300000000000000000000000000A",
"A210030000000000000000000000000000000000300000000000000000000000000A",
"A000000000000000000000000000000000000000333300000000000000000000000A",
"A000000000000000000000000000000000000000000300000000000000000000000A",
"A000000000000000000000000000000000000000000300000000000000000000000A",
"A000000000000000000000000000000000000000000300000000000000000000000A",
"A012333323333333233333333333333333333333333333333333333333333000000A",
"A000030000000000000000000000000000000000000000000000000000003000000A",
"A000030000000000000000000000000000000000000000000000000300003000000A",
"A000030000000000000000000000000000000000000000000000000300003000000A",
"A210030000000000000000000000000000000000000000000000000330003000000A",
"A000030000000000000000000000000000000000000000000000000300003000000A",
"A000030000000000000000000000000000000000000000000000000300003000000A",
"A000230000000000000000000000000000000000000000000000000300003000000A",
"A000030000000000000000000000000000000000000000000000000300003000000A",
"A000030000000000000000000000000000000000000000000000000300003000000A",
"A000030000000000000000000000000000000000000000000000000300003000000A",
"A000000000000000000000000000003333333333333333333333333333333000000A",
"A000000000000000000000000000000000000000000000000000000000000000000A",
"A000000000000000000000000000000000000000000000000000000000000000000A",
"A000000000000000000000000000000000000000000000000000000000000000000A",
"AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA",
};
static const char **z = z1;
if (x >= 0 && x < xMax && y >= 0 && y < yMax) {
int ret = z[x][y];
return ret;
}
return z[0][0];
}
};
struct CNodeKey1 {
int m_x;
int m_y;
Trackdir m_td;
DiagDirection m_exitdir;
CNodeKey1() : m_x(0), m_y(0), m_td(INVALID_TRACKDIR) {}
int CalcHash() const {return m_x | (m_y << 5) | (m_td << 10);}
bool operator == (const CNodeKey1& other) const {return (m_x == other.m_x) && (m_y == other.m_y) && (m_td == other.m_td);}
};
struct CNodeKey2 : public CNodeKey1
{
int CalcHash() const {return m_x | (m_y << 5) | (m_exitdir << 10);}
bool operator == (const CNodeKey1& other) const {return (m_x == other.m_x) && (m_y == other.m_y) && (m_exitdir == other.m_exitdir);}
};
template <class Tkey_>
struct CTestYapfNodeT {
typedef Tkey_ Key;
typedef CTestYapfNodeT<Tkey_> Node;
Tkey_ m_key;
CTestYapfNodeT *m_parent;
int m_cost;
int m_estimate;
CTestYapfNodeT *m_next;
CTestYapfNodeT(CTestYapfNodeT* parent = NULL) : m_parent(parent), m_cost(0), m_estimate(0), m_next(NULL) {}
const Tkey_& GetKey() const {return m_key;}
int GetCost() {return m_cost;}
int GetCostEstimate() {return m_estimate;}
bool operator < (const CTestYapfNodeT& other) const {return m_estimate < other.m_estimate;}
CTestYapfNodeT* GetHashNext() {return m_next;}
void SetHashNext(CTestYapfNodeT* next) {m_next = next;}
};
typedef CTestYapfNodeT<CNodeKey1> CYapfNode1;
typedef CTestYapfNodeT<CNodeKey2> CYapfNode2;
template <class Types>
struct CYapfTestBaseT
{
typedef typename Types::Tpf Tpf; ///< the pathfinder class (derived from THIS class)
typedef typename Types::NodeList::Titem Node; ///< this will be our node type
typedef typename Node::Key Key; ///< key to hash tables
typedef typename Types::Map Map;
int m_x1, m_y1;
int m_x2, m_y2;
Trackdir m_td1;
CYapfTestBaseT()
: m_x1(0), m_y1(0), m_x2(0), m_y2(0), m_td1(INVALID_TRACKDIR)
{
}
void Set(int x1, int y1, int x2, int y2, Trackdir td1)
{
m_x1 = x1;
m_y1 = y1;
m_x2 = x2;
m_y2 = y2;
m_td1 = td1;
}
/// to access inherited path finder
Tpf& Yapf() {return *static_cast<Tpf*>(this);}
FORCEINLINE char TransportTypeChar() const {return 'T';}
/** Called by YAPF to move from the given node to the next tile. For each
* reachable trackdir on the new tile creates new node, initializes it
* and adds it to the open list by calling Yapf().AddNewNode(n) */
FORCEINLINE void PfFollowNode(Node& org)
{
int x_org = org.m_key.m_x;
int y_org = org.m_key.m_y;
int z_org = Map::MapZ(x_org, y_org);
DiagDirection exitdir = TrackdirToExitdir(org.m_key.m_td);
TileIndexDiffC diff = TileIndexDiffCByDir(exitdir);
int x_new = x_org + diff.x;
int y_new = y_org + diff.y;
int z_new = Map::MapZ(x_new, y_new);
int z_diff = z_new - z_org;
if (abs(z_diff) > 1) return;
TrackdirBits trackdirs = DiagdirReachesTrackdirs(exitdir);
TrackdirBits trackdirs90 = TrackdirCrossesTrackdirs(org.m_key.m_td);
trackdirs &= (TrackdirBits)~(int)trackdirs90;
while (trackdirs != TRACKDIR_BIT_NONE) {
Trackdir td_new = (Trackdir)FindFirstBit2x64(trackdirs);
trackdirs = (TrackdirBits)KillFirstBit2x64(trackdirs);
Node& n = Yapf().CreateNewNode();
n.m_key.m_x = x_new;
n.m_key.m_y = y_new;
n.m_key.m_td = td_new;
n.m_key.m_exitdir = TrackdirToExitdir(n.m_key.m_td);
n.m_parent = &org;
Yapf().AddNewNode(n);
}
}
/// Called when YAPF needs to place origin nodes into open list
FORCEINLINE void PfSetStartupNodes()
{
Node& n1 = Yapf().CreateNewNode();
n1.m_key.m_x = m_x1;
n1.m_key.m_y = m_y1;
n1.m_key.m_td = m_td1;
n1.m_key.m_exitdir = TrackdirToExitdir(n1.m_key.m_td);
Yapf().AddStartupNode(n1);
}
/** Called by YAPF to calculate the cost from the origin to the given node.
* Calculates only the cost of given node, adds it to the parent node cost
* and stores the result into Node::m_cost member */
FORCEINLINE bool PfCalcCost(Node& n)
{
// base tile cost depending on distance
int c = IsDiagonalTrackdir(n.m_key.m_td) ? 10 : 7;
// additional penalty for curve
if (n.m_parent != NULL && n.m_key.m_td != n.m_parent->m_key.m_td) c += 3;
// z-difference cost
int z_new = Map::MapZ(n.m_key.m_x, n.m_key.m_y);
int z_old = Map::MapZ(n.m_parent->m_key.m_x, n.m_parent->m_key.m_y);
if (z_new > z_old) n.m_cost += (z_new - z_old) * 10;
// apply it
n.m_cost = n.m_parent->m_cost + c;
return true;
}
/** Called by YAPF to calculate cost estimate. Calculates distance to the destination
* adds it to the actual cost from origin and stores the sum to the Node::m_estimate */
FORCEINLINE bool PfCalcEstimate(Node& n)
{
int dx = abs(n.m_key.m_x - m_x2);
int dy = abs(n.m_key.m_y - m_y2);
int dd = min(dx, dy);
int dxy = abs(dx - dy);
int d = 14 * dd + 10 * dxy;
n.m_estimate = n.m_cost + d /*+ d / 4*/;
return true;
}
/// Called by YAPF to detect if node ends in the desired destination
FORCEINLINE bool PfDetectDestination(Node& n)
{
bool bDest = (n.m_key.m_x == m_x2) && (n.m_key.m_y == m_y2);
return bDest;
}
static int stTestAstar(bool silent)
{
Tpf pf;
pf.Set(3, 3, 20, 56, TRACKDIR_X_NE);
int ret = pf.TestAstar(silent);
return ret;
}
int TestAstar(bool silent)
{
CPerformanceTimer pc;
pc.Start();
bool bRet = Yapf().FindPath(NULL);
pc.Stop();
if (!bRet) return 1;
typedef CFixedSizeArrayT<int, 1024> Row;
typedef CFixedSizeArrayT<Row, 1024> Box;
Box box;
{
for (int x = 0; x < Map::xMax; x++) {
Row& row = box.Add();
for (int y = 0; y < Map::yMax; y++) {
row.Add() = Map::MapZ(x, y);
}
}
}
int nPathTiles = 0;
{
for (Node* pNode = &Yapf().GetBestNode(); pNode != NULL; pNode = pNode->m_parent) {
box[pNode->m_key.m_x][pNode->m_key.m_y] = '.';
nPathTiles++;
}
}
{
printf("\n\n");
for (int x = 0; x < Map::xMax; x++) {
for (int y = 0; y < Map::yMax; y++) {
printf("%c", box[x][y]);
}
printf("\n");
}
}
{
printf("\n");
printf("Path Tiles: %6d\n", nPathTiles);
// printf("Closed nodes: %6d\n", pf.m_nodes.ClosedCount());
// printf("Open nodes: %6d\n", pf.m_nodes.OpenCount());
// printf("A-star rounds: %6d\n", pf.m_num_steps);
}
int total_time = pc.Get(1000000);
if (total_time != 0)
printf("Total time: %6d us\n", pc.Get(1000000));
printf("\n");
{
int nCnt = Yapf().m_nodes.TotalCount();
for (int i = 0; i < nCnt; i++) {
Node& n = Yapf().m_nodes.ItemAt(i);
int& z = box[n.m_key.m_x][n.m_key.m_y];
z = (z < 'a') ? 'a' : (z + 1);
}
}
{
for (int x = 0; x < Map::xMax; x++) {
for (int y = 0; y < Map::yMax; y++) {
printf("%c", box[x][y]);
}
printf("\n");
}
}
return 0;
}
};
struct CDummy1 {};
struct CDummy2 {};
struct CDummy3 {};
template <class Tpf_, class Tnode_list, class Tmap>
struct CYapf_TypesT
{
typedef CYapf_TypesT<Tpf_, Tnode_list, Tmap> Types;
typedef Tpf_ Tpf;
typedef Tnode_list NodeList;
typedef Tmap Map;
typedef CYapfBaseT<Types> PfBase;
typedef CYapfTestBaseT<Types> PfFollow;
typedef CDummy1 PfOrigin;
typedef CDummy2 PfDestination;
typedef CYapfSegmentCostCacheNoneT<Types> PfCache;
typedef CDummy3 PfCost;
};
typedef CNodeList_HashTableT<CYapfNode1, 12, 16> CNodeList1;
typedef CNodeList_HashTableT<CYapfNode2, 12, 16> CNodeList2;
struct CTestYapf1
: public CYapfT<CYapf_TypesT<CTestYapf1, CNodeList1, CYapfMap1> >
{
};
struct CTestYapf2
: public CYapfT<CYapf_TypesT<CTestYapf2, CNodeList2, CYapfMap1> >
{
};
|
