|
1 /* $Id$ */ |
|
2 |
|
3 #ifndef YAPF_BASE_HPP |
|
4 #define YAPF_BASE_HPP |
|
5 |
|
6 EXTERN_C_BEGIN |
|
7 #include "../debug.h" |
|
8 EXTERN_C_END |
|
9 |
|
10 #include "fixedsizearray.hpp" |
|
11 #include "blob.hpp" |
|
12 #include "nodelist.hpp" |
|
13 |
|
14 extern int _total_pf_time_us; |
|
15 |
|
16 /** CYapfBaseT - A-star type path finder base class. |
|
17 Derive your own pathfinder from it. You must provide the following template argument: |
|
18 Types - used as collection of local types used in pathfinder |
|
19 |
|
20 Requirements for the Types struct: |
|
21 ---------------------------------- |
|
22 The following types must be defined in the 'Types' argument: |
|
23 - Types::Tpf - your pathfinder derived from CYapfBaseT |
|
24 - Types::NodeList - open/closed node list (look at CNodeList_HashTableT) |
|
25 NodeList needs to have defined local type Titem - defines the pathfinder node type. |
|
26 Node needs to define local type Key - the node key in the collection () |
|
27 |
|
28 For node list you can use template class CNodeList_HashTableT, for which |
|
29 you need to declare only your node type. Look at test_yapf.h for an example. |
|
30 |
|
31 |
|
32 Requrements to your pathfinder class derived from CYapfBaseT: |
|
33 ------------------------------------------------------------- |
|
34 Your pathfinder derived class needs to implement following methods: |
|
35 FORCEINLINE void PfSetStartupNodes() |
|
36 FORCEINLINE void PfFollowNode(Node& org) |
|
37 FORCEINLINE bool PfCalcCost(Node& n) |
|
38 FORCEINLINE bool PfCalcEstimate(Node& n) |
|
39 FORCEINLINE bool PfDetectDestination(Node& n) |
|
40 |
|
41 For more details about those methods, look at the end of CYapfBaseT |
|
42 declaration. There are some examples. For another example look at |
|
43 test_yapf.h (part or unittest project). |
|
44 */ |
|
45 template <class Types> |
|
46 class CYapfBaseT { |
|
47 public: |
|
48 typedef typename Types::Tpf Tpf; |
|
49 typedef typename Types::NodeList NodeList; ///< our node list |
|
50 typedef typename NodeList::Titem Node; ///< this will be our node type |
|
51 typedef typename Node::Key Key; ///< key to hash tables |
|
52 |
|
53 |
|
54 NodeList m_nodes; ///< node list multi-container |
|
55 protected: |
|
56 Node* m_pBestDestNode; ///< pointer to the destination node found at last round |
|
57 Node* m_pBestIntermediateNode; |
|
58 const YapfSettings *m_settings; |
|
59 int m_max_search_nodes; |
|
60 Vehicle* m_veh; |
|
61 |
|
62 int m_stats_cost_calcs; |
|
63 int m_stats_cache_hits; |
|
64 |
|
65 public: |
|
66 CPerformanceTimer m_perf_cost; |
|
67 CPerformanceTimer m_perf_slope_cost; |
|
68 CPerformanceTimer m_perf_ts_cost; |
|
69 CPerformanceTimer m_perf_other_cost; |
|
70 |
|
71 public: |
|
72 int m_num_steps; ///< this is there for debugging purposes (hope it doesn't hurt) |
|
73 |
|
74 public: |
|
75 // default constructor |
|
76 FORCEINLINE CYapfBaseT() |
|
77 : m_pBestDestNode(NULL) |
|
78 , m_pBestIntermediateNode(NULL) |
|
79 #if defined(UNITTEST) |
|
80 , m_settings(NULL) |
|
81 , m_max_search_nodes(100000) |
|
82 #else |
|
83 , m_settings(&_patches.yapf) |
|
84 , m_max_search_nodes(PfGetSettings().max_search_nodes) |
|
85 #endif |
|
86 , m_veh(NULL) |
|
87 , m_stats_cost_calcs(0) |
|
88 , m_stats_cache_hits(0) |
|
89 , m_num_steps(0) |
|
90 { |
|
91 } |
|
92 |
|
93 ~CYapfBaseT() {} |
|
94 |
|
95 protected: |
|
96 FORCEINLINE Tpf& Yapf() {return *static_cast<Tpf*>(this);} |
|
97 |
|
98 public: |
|
99 FORCEINLINE const YapfSettings& PfGetSettings() const |
|
100 { |
|
101 return *m_settings; |
|
102 } |
|
103 |
|
104 /** Main pathfinder routine: |
|
105 - set startup node(s) |
|
106 - main loop that stops if: |
|
107 - the destination was found |
|
108 - or the open list is empty (no route to destination). |
|
109 - or the maximum amount of loops reached - m_max_search_nodes (default = 10000) |
|
110 @return true if the path was found */ |
|
111 inline bool FindPath(Vehicle* v) |
|
112 { |
|
113 m_veh = v; |
|
114 |
|
115 CPerformanceTimer perf; |
|
116 perf.Start(); |
|
117 Yapf().PfSetStartupNodes(); |
|
118 |
|
119 while (true) { |
|
120 m_num_steps++; |
|
121 Node& n = m_nodes.GetBestOpenNode(); |
|
122 if (&n == NULL) |
|
123 break; |
|
124 |
|
125 // if the best open node was worse than the best path found, we can finish |
|
126 if (m_pBestDestNode != NULL && m_pBestDestNode->GetCost() < n.GetCostEstimate()) |
|
127 break; |
|
128 |
|
129 Yapf().PfFollowNode(n); |
|
130 if (m_max_search_nodes == 0 || m_nodes.ClosedCount() < m_max_search_nodes) { |
|
131 m_nodes.PopOpenNode(n.GetKey()); |
|
132 m_nodes.InsertClosedNode(n); |
|
133 } else { |
|
134 m_pBestDestNode = m_pBestIntermediateNode; |
|
135 break; |
|
136 } |
|
137 } |
|
138 bool bDestFound = (m_pBestDestNode != NULL); |
|
139 |
|
140 int16 veh_idx = (m_veh != NULL) ? m_veh->unitnumber : 0; |
|
141 |
|
142 // if (veh_idx != 433) return bDestFound; |
|
143 |
|
144 perf.Stop(); |
|
145 int t = perf.Get(1000000); |
|
146 _total_pf_time_us += t; |
|
147 char ttc = Yapf().TransportTypeChar(); |
|
148 float cache_hit_ratio = (float)m_stats_cache_hits / (float)(m_stats_cache_hits + m_stats_cost_calcs) * 100.0f; |
|
149 int cost = bDestFound ? m_pBestDestNode->m_cost : -1; |
|
150 int dist = bDestFound ? m_pBestDestNode->m_estimate - m_pBestDestNode->m_cost : -1; |
|
151 #ifdef UNITTEST |
|
152 printf("%c%c%4d-%6d us -%5d rounds -%4d open -%5d closed - CHR %4.1f%% - c/d(%d, %d) - c%d(sc%d, ts%d, o%d) -- \n", bDestFound ? '-' : '!', ttc, veh_idx, t, m_num_steps, m_nodes.OpenCount(), m_nodes.ClosedCount(), cache_hit_ratio, cost, dist, m_perf_cost.Get(1000000), m_perf_slope_cost.Get(1000000), m_perf_ts_cost.Get(1000000), m_perf_other_cost.Get(1000000)); |
|
153 #else |
|
154 DEBUG(yapf, 1)("[YAPF][YAPF%c]%c%4d- %d us - %d rounds - %d open - %d closed - CHR %4.1f%% - c%d(sc%d, ts%d, o%d) -- ", ttc, bDestFound ? '-' : '!', veh_idx, t, m_num_steps, m_nodes.OpenCount(), m_nodes.ClosedCount(), cache_hit_ratio, cost, dist, m_perf_cost.Get(1000000), m_perf_slope_cost.Get(1000000), m_perf_ts_cost.Get(1000000), m_perf_other_cost.Get(1000000)); |
|
155 #endif |
|
156 return bDestFound; |
|
157 } |
|
158 |
|
159 /** If path was found return the best node that has reached the destination. Otherwise |
|
160 return the best visited node (which was nearest to the destination). |
|
161 */ |
|
162 FORCEINLINE Node& GetBestNode() |
|
163 { |
|
164 return (m_pBestDestNode != NULL) ? *m_pBestDestNode : *m_pBestIntermediateNode; |
|
165 } |
|
166 |
|
167 /** Calls NodeList::CreateNewNode() - allocates new node that can be filled and used |
|
168 as argument for AddStartupNode() or AddNewNode() |
|
169 */ |
|
170 FORCEINLINE Node& CreateNewNode() |
|
171 { |
|
172 Node& node = *m_nodes.CreateNewNode(); |
|
173 return node; |
|
174 } |
|
175 |
|
176 /** Add new node (created by CreateNewNode and filled with data) into open list */ |
|
177 FORCEINLINE void AddStartupNode(Node& n) |
|
178 { |
|
179 Yapf().PfNodeCacheFetch(n); |
|
180 m_nodes.InsertOpenNode(n); |
|
181 } |
|
182 |
|
183 /** add multiple nodes - direct children of the given node */ |
|
184 FORCEINLINE void AddMultipleNodes(Node* parent, TileIndex tile, TrackdirBits td_bits) |
|
185 { |
|
186 for (TrackdirBits rtds = td_bits; rtds != TRACKDIR_BIT_NONE; rtds = (TrackdirBits)KillFirstBit2x64(rtds)) { |
|
187 Trackdir td = (Trackdir)FindFirstBit2x64(rtds); |
|
188 Node& n = Yapf().CreateNewNode(); |
|
189 n.Set(parent, tile, td); |
|
190 Yapf().AddNewNode(n); |
|
191 } |
|
192 } |
|
193 |
|
194 /** AddNewNode() - called by Tderived::PfFollowNode() for each child node. |
|
195 Nodes are evaluated here and added into open list */ |
|
196 void AddNewNode(Node& n) |
|
197 { |
|
198 // evaluate the node |
|
199 bool bCached = Yapf().PfNodeCacheFetch(n); |
|
200 if (!bCached) { |
|
201 m_stats_cost_calcs++; |
|
202 } else { |
|
203 m_stats_cache_hits++; |
|
204 } |
|
205 |
|
206 bool bValid = Yapf().PfCalcCost(n); |
|
207 |
|
208 if (bCached) { |
|
209 Yapf().PfNodeCacheFlush(n); |
|
210 } |
|
211 |
|
212 if (bValid) bValid = Yapf().PfCalcEstimate(n); |
|
213 |
|
214 // have the cost or estimate callbacks marked this node as invalid? |
|
215 if (!bValid) return; |
|
216 |
|
217 // detect the destination |
|
218 bool bDestination = Yapf().PfDetectDestination(n); |
|
219 if (bDestination) { |
|
220 if (m_pBestDestNode == NULL || n < *m_pBestDestNode) { |
|
221 m_pBestDestNode = &n; |
|
222 } |
|
223 m_nodes.FoundBestNode(n); |
|
224 return; |
|
225 } |
|
226 |
|
227 if (m_max_search_nodes > 0 && (m_pBestIntermediateNode == NULL || (m_pBestIntermediateNode->GetCostEstimate() - m_pBestIntermediateNode->GetCost()) > (n.GetCostEstimate() - n.GetCost()))) { |
|
228 m_pBestIntermediateNode = &n; |
|
229 } |
|
230 |
|
231 // check new node against open list |
|
232 Node& openNode = m_nodes.FindOpenNode(n.GetKey()); |
|
233 if (&openNode != NULL) { |
|
234 // another node exists with the same key in the open list |
|
235 // is it better than new one? |
|
236 if (n.GetCostEstimate() < openNode.GetCostEstimate()) { |
|
237 // update the old node by value from new one |
|
238 m_nodes.PopOpenNode(n.GetKey()); |
|
239 openNode = n; |
|
240 // add the updated old node back to open list |
|
241 m_nodes.InsertOpenNode(openNode); |
|
242 } |
|
243 return; |
|
244 } |
|
245 |
|
246 // check new node against closed list |
|
247 Node& closedNode = m_nodes.FindClosedNode(n.GetKey()); |
|
248 if (&closedNode != NULL) { |
|
249 // another node exists with the same key in the closed list |
|
250 // is it better than new one? |
|
251 int node_est = n.GetCostEstimate(); |
|
252 int closed_est = closedNode.GetCostEstimate(); |
|
253 if (node_est < closed_est) { |
|
254 // If this assert occurs, you have probably problem in |
|
255 // your Tderived::PfCalcCost() or Tderived::PfCalcEstimate(). |
|
256 // The problem could be: |
|
257 // - PfCalcEstimate() gives too large numbers |
|
258 // - PfCalcCost() gives too small numbers |
|
259 // - You have used negative cost penalty in some cases (cost bonus) |
|
260 assert(0); |
|
261 |
|
262 return; |
|
263 } |
|
264 return; |
|
265 } |
|
266 // the new node is really new |
|
267 // add it to the open list |
|
268 m_nodes.InsertOpenNode(n); |
|
269 } |
|
270 |
|
271 Vehicle* GetVehicle() const {return m_veh;} |
|
272 |
|
273 // methods that should be implemented at derived class Types::Tpf (derived from CYapfBaseT) |
|
274 |
|
275 #if 0 |
|
276 /** Example: PfSetStartupNodes() - set source (origin) nodes */ |
|
277 FORCEINLINE void PfSetStartupNodes() |
|
278 { |
|
279 // example: |
|
280 Node& n1 = *base::m_nodes.CreateNewNode(); |
|
281 . |
|
282 . // setup node members here |
|
283 . |
|
284 base::m_nodes.InsertOpenNode(n1); |
|
285 } |
|
286 |
|
287 /** Example: PfFollowNode() - set following (child) nodes of the given node */ |
|
288 FORCEINLINE void PfFollowNode(Node& org) |
|
289 { |
|
290 for (each follower of node org) { |
|
291 Node& n = *base::m_nodes.CreateNewNode(); |
|
292 . |
|
293 . // setup node members here |
|
294 . |
|
295 n.m_parent = &org; // set node's parent to allow back tracking |
|
296 AddNewNode(n); |
|
297 } |
|
298 } |
|
299 |
|
300 /** Example: PfCalcCost() - set path cost from origin to the given node */ |
|
301 FORCEINLINE bool PfCalcCost(Node& n) |
|
302 { |
|
303 // evaluate last step cost |
|
304 int cost = ...; |
|
305 // set the node cost as sum of parent's cost and last step cost |
|
306 n.m_cost = n.m_parent->m_cost + cost; |
|
307 return true; // true if node is valid follower (i.e. no obstacle was found) |
|
308 } |
|
309 |
|
310 /** Example: PfCalcEstimate() - set path cost estimate from origin to the target through given node */ |
|
311 FORCEINLINE bool PfCalcEstimate(Node& n) |
|
312 { |
|
313 // evaluate the distance to our destination |
|
314 int distance = ...; |
|
315 // set estimate as sum of cost from origin + distance to the target |
|
316 n.m_estimate = n.m_cost + distance; |
|
317 return true; // true if node is valid (i.e. not too far away :) |
|
318 } |
|
319 |
|
320 /** Example: PfDetectDestination() - return true if the given node is our destination */ |
|
321 FORCEINLINE bool PfDetectDestination(Node& n) |
|
322 { |
|
323 bool bDest = (n.m_key.m_x == m_x2) && (n.m_key.m_y == m_y2); |
|
324 return bDest; |
|
325 } |
|
326 #endif |
|
327 }; |
|
328 |
|
329 #endif /* YAPF_BASE_HPP */ |