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