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