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;           ///< the pathfinder class (derived from THIS class)

    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; ///< here should be node closest to the destination if path not found

    58

	const YapfSettings  *m_settings;           ///< current settings (_patches.yapf)

    59

	int                  m_max_search_nodes;   ///< maximum number of nodes we are allowed to visit before we give up

    60

	const Vehicle*       m_veh;                ///< vehicle that we are trying to drive

    61

    62

	int                  m_stats_cost_calcs;   ///< stats - how many node's costs were calculated

    63

	int                  m_stats_cache_hits;   ///< stats - how many node's costs were reused from cache

    64

    65

public:

    66

	CPerformanceTimer    m_perf_cost;          ///< stats - total CPU time of this run

    67

	CPerformanceTimer    m_perf_slope_cost;    ///< stats - slope calculation CPU time

    68

	CPerformanceTimer    m_perf_ts_cost;       ///< stats - GetTrackStatus() CPU time

    69

	CPerformanceTimer    m_perf_other_cost;    ///< stats - other CPU time

    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

	/// default destructor

    94

	~CYapfBaseT() {}

    95

    96

protected:

    97

	/// to access inherited path finder

    98

	FORCEINLINE Tpf& Yapf() {return *static_cast<Tpf*>(this);}

    99

   100

public:

   101

	/// return current settings (can be custom - player based - but later)

   102

	FORCEINLINE const YapfSettings& PfGetSettings() const

   103

	{

   104

		return *m_settings;

   105

	}

   106

   107

	/** Main pathfinder routine:

   108

			 - set startup node(s)

   109

			 - main loop that stops if:

   110

					- the destination was found

   111

					- or the open list is empty (no route to destination).

   112

					- or the maximum amount of loops reached - m_max_search_nodes (default = 10000)

   113

			@return true if the path was found */

   114

	inline bool FindPath(const Vehicle* v)

   115

	{

   116

		m_veh = v;

   117

   118

		CPerformanceTimer perf;

   119

		perf.Start();

   120

		Yapf().PfSetStartupNodes();

   121

   122

		while (true) {

   123

			m_num_steps++;

   124

			Node& n = m_nodes.GetBestOpenNode();

   125

			if (&n == NULL)

   126

				break;

   127

   128

			// if the best open node was worse than the best path found, we can finish

   129

			if (m_pBestDestNode != NULL && m_pBestDestNode->GetCost() < n.GetCostEstimate())

   130

				break;

   131

   132

			Yapf().PfFollowNode(n);

   133

			if (m_max_search_nodes == 0 || m_nodes.ClosedCount() < m_max_search_nodes) {

   134

				m_nodes.PopOpenNode(n.GetKey());

   135

				m_nodes.InsertClosedNode(n);

   136

			} else {

   137

				m_pBestDestNode = m_pBestIntermediateNode;

   138

				break;

   139

			}

   140

		}

   141

		bool bDestFound = (m_pBestDestNode != NULL);

   142

   143

		int16 veh_idx = (m_veh != NULL) ? m_veh->unitnumber : 0;

   144

   145

//		if (veh_idx != 433) return bDestFound;

   146

   147

		perf.Stop();

   148

		int t = perf.Get(1000000);

   149

		_total_pf_time_us += t;

   150

		char ttc = Yapf().TransportTypeChar();

   151

		float cache_hit_ratio = (float)m_stats_cache_hits / (float)(m_stats_cache_hits + m_stats_cost_calcs) * 100.0f;

   152

		int cost = bDestFound ? m_pBestDestNode->m_cost : -1;

   153

		int dist = bDestFound ? m_pBestDestNode->m_estimate - m_pBestDestNode->m_cost : -1;

   154

#ifdef UNITTEST

   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));

   156

#else

   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));

   158

#endif

   159

		return bDestFound;

   160

	}

   161

   162

	/** If path was found return the best node that has reached the destination. Otherwise

   163

			return the best visited node (which was nearest to the destination).

   164

	*/

   165

	FORCEINLINE Node& GetBestNode()

   166

	{

   167

		return (m_pBestDestNode != NULL) ? *m_pBestDestNode : *m_pBestIntermediateNode;

   168

	}

   169

   170

	/** Calls NodeList::CreateNewNode() - allocates new node that can be filled and used

   171

			as argument for AddStartupNode() or AddNewNode()

   172

	*/

   173

	FORCEINLINE Node& CreateNewNode()

   174

	{

   175

		Node& node = *m_nodes.CreateNewNode();

   176

		return node;

   177

	}

   178

   179

	/** Add new node (created by CreateNewNode and filled with data) into open list */

   180

	FORCEINLINE void AddStartupNode(Node& n)

   181

	{

   182

		Yapf().PfNodeCacheFetch(n);

   183

		m_nodes.InsertOpenNode(n);

   184

	}

   185

   186

	/** add multiple nodes - direct children of the given node */

   187

	FORCEINLINE void AddMultipleNodes(Node* parent, TileIndex tile, TrackdirBits td_bits)

   188

	{

   189

		bool is_choice = (KillFirstBit2x64(td_bits) != 0);

   190

		for (TrackdirBits rtds = td_bits; rtds != TRACKDIR_BIT_NONE; rtds = (TrackdirBits)KillFirstBit2x64(rtds)) {

   191

			Trackdir td = (Trackdir)FindFirstBit2x64(rtds);

   192

			Node& n = Yapf().CreateNewNode();

   193

			n.Set(parent, tile, td, is_choice);

   194

			Yapf().AddNewNode(n);

   195

		}

   196

	}

   197

   198

	/** AddNewNode() - called by Tderived::PfFollowNode() for each child node.

   199

	    Nodes are evaluated here and added into open list */

   200

	void AddNewNode(Node& n)

   201

	{

   202

		// evaluate the node

   203

		bool bCached = Yapf().PfNodeCacheFetch(n);

   204

		if (!bCached) {

   205

			m_stats_cost_calcs++;

   206

		} else {

   207

			m_stats_cache_hits++;

   208

		}

   209

   210

		bool bValid = Yapf().PfCalcCost(n);

   211

   212

		if (bCached) {

   213

			Yapf().PfNodeCacheFlush(n);

   214

		}

   215

   216

		if (bValid) bValid = Yapf().PfCalcEstimate(n);

   217

   218

		// have the cost or estimate callbacks marked this node as invalid?

   219

		if (!bValid) return;

   220

   221

		// detect the destination

   222

		bool bDestination = Yapf().PfDetectDestination(n);

   223

		if (bDestination) {

   224

			if (m_pBestDestNode == NULL || n < *m_pBestDestNode) {

   225

				m_pBestDestNode = &n;

   226

			}

   227

			m_nodes.FoundBestNode(n);

   228

			return;

   229

		}

   230

   231

		if (m_max_search_nodes > 0 && (m_pBestIntermediateNode == NULL || (m_pBestIntermediateNode->GetCostEstimate() - m_pBestIntermediateNode->GetCost()) > (n.GetCostEstimate() - n.GetCost()))) {

   232

			m_pBestIntermediateNode = &n;

   233

		}

   234

   235

		// check new node against open list

   236

		Node& openNode = m_nodes.FindOpenNode(n.GetKey());

   237

		if (&openNode != NULL) {

   238

			// another node exists with the same key in the open list

   239

			// is it better than new one?

   240

			if (n.GetCostEstimate() < openNode.GetCostEstimate()) {

   241

				// update the old node by value from new one

   242

				m_nodes.PopOpenNode(n.GetKey());

   243

				openNode = n;

   244

				// add the updated old node back to open list

   245

				m_nodes.InsertOpenNode(openNode);

   246

			}

   247

			return;

   248

		}

   249

   250

		// check new node against closed list

   251

		Node& closedNode = m_nodes.FindClosedNode(n.GetKey());

   252

		if (&closedNode != NULL) {

   253

			// another node exists with the same key in the closed list

   254

			// is it better than new one?

   255

			int node_est = n.GetCostEstimate();

   256

			int closed_est = closedNode.GetCostEstimate();

   257

			if (node_est < closed_est) {

   258

				// If this assert occurs, you have probably problem in

   259

				// your Tderived::PfCalcCost() or Tderived::PfCalcEstimate().

   260

				// The problem could be:

   261

				//  - PfCalcEstimate() gives too large numbers

   262

				//  - PfCalcCost() gives too small numbers

   263

				//  - You have used negative cost penalty in some cases (cost bonus)

   264

				assert(0);

   265

   266

				return;

   267

			}

   268

			return;

   269

		}

   270

		// the new node is really new

   271

		// add it to the open list

   272

		m_nodes.InsertOpenNode(n);

   273

	}

   274

   275

	const Vehicle* GetVehicle() const {return m_veh;}

   276

   277

	// methods that should be implemented at derived class Types::Tpf (derived from CYapfBaseT)

   278

   279

#if 0

   280

	/** Example: PfSetStartupNodes() - set source (origin) nodes */

   281

	FORCEINLINE void PfSetStartupNodes()

   282

	{

   283

		// example:

   284

		Node& n1 = *base::m_nodes.CreateNewNode();

   285

		.

   286

		. // setup node members here

   287

		.

   288

		base::m_nodes.InsertOpenNode(n1);

   289

	}

   290

   291

	/** Example: PfFollowNode() - set following (child) nodes of the given node */

   292

	FORCEINLINE void PfFollowNode(Node& org)

   293

	{

   294

		for (each follower of node org) {

   295

			Node& n = *base::m_nodes.CreateNewNode();

   296

			.

   297

			. // setup node members here

   298

			.

   299

			n.m_parent   = &org; // set node's parent to allow back tracking

   300

			AddNewNode(n);

   301

		}

   302

	}

   303

   304

	/** Example: PfCalcCost() - set path cost from origin to the given node */

   305

	FORCEINLINE bool PfCalcCost(Node& n)

   306

	{

   307

		// evaluate last step cost

   308

		int cost = ...;

   309

		// set the node cost as sum of parent's cost and last step cost

   310

		n.m_cost = n.m_parent->m_cost + cost;

   311

		return true; // true if node is valid follower (i.e. no obstacle was found)

   312

	}

   313

   314

	/** Example: PfCalcEstimate() - set path cost estimate from origin to the target through given node */

   315

	FORCEINLINE bool PfCalcEstimate(Node& n)

   316

	{

   317

		// evaluate the distance to our destination

   318

		int distance = ...;

   319

		// set estimate as sum of cost from origin + distance to the target

   320

		n.m_estimate = n.m_cost + distance;

   321

		return true; // true if node is valid (i.e. not too far away :)

   322

	}

   323

   324

	/** Example: PfDetectDestination() - return true if the given node is our destination */

   325

	FORCEINLINE bool PfDetectDestination(Node& n)

   326

	{

   327

		bool bDest = (n.m_key.m_x == m_x2) && (n.m_key.m_y == m_y2);

   328

		return bDest;

   329

	}

   330

#endif

   331

};

   332

   333

#endif /* YAPF_BASE_HPP */