1

/* $Id$ */

     2

     3

/** @file yapf_road.cpp The road pathfinding. */

     4

     5

#include "../stdafx.h"

     6

#include "../depot_base.h"

     7

     8

#include "yapf.hpp"

     9

#include "yapf_node_road.hpp"

    10

    11

    12

template <class Types>

    13

class CYapfCostRoadT

    14

{

    15

public:

    16

	typedef typename Types::Tpf Tpf; ///< pathfinder (derived from THIS class)

    17

	typedef typename Types::TrackFollower TrackFollower; ///< track follower helper

    18

	typedef typename Types::NodeList::Titem Node; ///< this will be our node type

    19

	typedef typename Node::Key Key;    ///< key to hash tables

    20

    21

protected:

    22

	/// to access inherited path finder

    23

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

    24

    25

	int SlopeCost(TileIndex tile, TileIndex next_tile, Trackdir trackdir)

    26

	{

    27

		// height of the center of the current tile

    28

		int x1 = TileX(tile) * TILE_SIZE;

    29

		int y1 = TileY(tile) * TILE_SIZE;

    30

		int z1 = GetSlopeZ(x1 + TILE_SIZE / 2, y1 + TILE_SIZE / 2);

    31

    32

		// height of the center of the next tile

    33

		int x2 = TileX(next_tile) * TILE_SIZE;

    34

		int y2 = TileY(next_tile) * TILE_SIZE;

    35

		int z2 = GetSlopeZ(x2 + TILE_SIZE / 2, y2 + TILE_SIZE / 2);

    36

    37

		if (z2 - z1 > 1) {

    38

			/* Slope up */

    39

			return Yapf().PfGetSettings().road_slope_penalty;

    40

		}

    41

		return 0;

    42

	}

    43

    44

	/** return one tile cost */

    45

	FORCEINLINE int OneTileCost(TileIndex tile, Trackdir trackdir)

    46

	{

    47

		int cost = 0;

    48

		// set base cost

    49

		if (IsDiagonalTrackdir(trackdir)) {

    50

			cost += YAPF_TILE_LENGTH;

    51

			switch (GetTileType(tile)) {

    52

				case MP_ROAD:

    53

					/* Increase the cost for level crossings */

    54

					if (IsLevelCrossing(tile))

    55

						cost += Yapf().PfGetSettings().road_crossing_penalty;

    56

					break;

    57

				case MP_STATION:

    58

					if (IsDriveThroughStopTile(tile))

    59

						cost += Yapf().PfGetSettings().road_stop_penalty;

    60

					break;

    61

    62

				default:

    63

					break;

    64

			}

    65

		} else {

    66

			// non-diagonal trackdir

    67

			cost = YAPF_TILE_CORNER_LENGTH + Yapf().PfGetSettings().road_curve_penalty;

    68

		}

    69

		return cost;

    70

	}

    71

    72

public:

    73

	/** Called by YAPF to calculate the cost from the origin to the given node.

    74

	 *  Calculates only the cost of given node, adds it to the parent node cost

    75

	 *  and stores the result into Node::m_cost member */

    76

	FORCEINLINE bool PfCalcCost(Node& n, const TrackFollower *tf)

    77

	{

    78

		int segment_cost = 0;

    79

		// start at n.m_key.m_tile / n.m_key.m_td and walk to the end of segment

    80

		TileIndex tile = n.m_key.m_tile;

    81

		Trackdir trackdir = n.m_key.m_td;

    82

		while (true) {

    83

			// base tile cost depending on distance between edges

    84

			segment_cost += Yapf().OneTileCost(tile, trackdir);

    85

    86

			const Vehicle *v = Yapf().GetVehicle();

    87

			// we have reached the vehicle's destination - segment should end here to avoid target skipping

    88

			if (v->current_order.IsType(OT_GOTO_STATION) && tile == v->dest_tile) break;

    89

    90

			// stop if we have just entered the depot

    91

			if (IsRoadDepotTile(tile) && trackdir == DiagDirToDiagTrackdir(ReverseDiagDir(GetRoadDepotDirection(tile)))) {

    92

				// next time we will reverse and leave the depot

    93

				break;

    94

			}

    95

    96

			// if there are no reachable trackdirs on new tile, we have end of road

    97

			TrackFollower F(Yapf().GetVehicle());

    98

			if (!F.Follow(tile, trackdir)) break;

    99

   100

			// if there are more trackdirs available & reachable, we are at the end of segment

   101

			if (KillFirstBit(F.m_new_td_bits) != TRACKDIR_BIT_NONE) break;

   102

   103

			Trackdir new_td = (Trackdir)FindFirstBit2x64(F.m_new_td_bits);

   104

   105

			// stop if RV is on simple loop with no junctions

   106

			if (F.m_new_tile == n.m_key.m_tile && new_td == n.m_key.m_td) return false;

   107

   108

			// if we skipped some tunnel tiles, add their cost

   109

			segment_cost += F.m_tiles_skipped * YAPF_TILE_LENGTH;

   110

   111

			// add hilly terrain penalty

   112

			segment_cost += Yapf().SlopeCost(tile, F.m_new_tile, trackdir);

   113

   114

			// add min/max speed penalties

   115

			int min_speed = 0;

   116

			int max_speed = F.GetSpeedLimit(&min_speed);

   117

			if (max_speed < v->max_speed) segment_cost += 1 * (v->max_speed - max_speed);

   118

			if (min_speed > v->max_speed) segment_cost += 10 * (min_speed - v->max_speed);

   119

   120

			// move to the next tile

   121

			tile = F.m_new_tile;

   122

			trackdir = new_td;

   123

		};

   124

   125

		// save end of segment back to the node

   126

		n.m_segment_last_tile = tile;

   127

		n.m_segment_last_td = trackdir;

   128

   129

		// save also tile cost

   130

		int parent_cost = (n.m_parent != NULL) ? n.m_parent->m_cost : 0;

   131

		n.m_cost = parent_cost + segment_cost;

   132

		return true;

   133

	}

   134

};

   135

   136

   137

template <class Types>

   138

class CYapfDestinationAnyDepotRoadT

   139

{

   140

public:

   141

	typedef typename Types::Tpf Tpf;                     ///< the pathfinder class (derived from THIS class)

   142

	typedef typename Types::TrackFollower TrackFollower;

   143

	typedef typename Types::NodeList::Titem Node;        ///< this will be our node type

   144

	typedef typename Node::Key Key;                      ///< key to hash tables

   145

   146

	/// to access inherited path finder

   147

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

   148

   149

	/// Called by YAPF to detect if node ends in the desired destination

   150

	FORCEINLINE bool PfDetectDestination(Node& n)

   151

	{

   152

		bool bDest = IsRoadDepotTile(n.m_segment_last_tile);

   153

		return bDest;

   154

	}

   155

   156

	/** Called by YAPF to calculate cost estimate. Calculates distance to the destination

   157

	 *  adds it to the actual cost from origin and stores the sum to the Node::m_estimate */

   158

	FORCEINLINE bool PfCalcEstimate(Node& n)

   159

	{

   160

		n.m_estimate = n.m_cost;

   161

		return true;

   162

	}

   163

};

   164

   165

   166

template <class Types>

   167

class CYapfDestinationTileRoadT

   168

{

   169

public:

   170

	typedef typename Types::Tpf Tpf;                     ///< the pathfinder class (derived from THIS class)

   171

	typedef typename Types::TrackFollower TrackFollower;

   172

	typedef typename Types::NodeList::Titem Node;        ///< this will be our node type

   173

	typedef typename Node::Key Key;                      ///< key to hash tables

   174

   175

protected:

   176

	TileIndex    m_destTile;

   177

	TrackdirBits m_destTrackdirs;

   178

   179

public:

   180

	void SetDestination(TileIndex tile, TrackdirBits trackdirs)

   181

	{

   182

		m_destTile = tile;

   183

		m_destTrackdirs = trackdirs;

   184

	}

   185

   186

protected:

   187

	/// to access inherited path finder

   188

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

   189

   190

public:

   191

	/// Called by YAPF to detect if node ends in the desired destination

   192

	FORCEINLINE bool PfDetectDestination(Node& n)

   193

	{

   194

		bool bDest = (n.m_segment_last_tile == m_destTile) && ((m_destTrackdirs & TrackdirToTrackdirBits(n.m_segment_last_td)) != TRACKDIR_BIT_NONE);

   195

		return bDest;

   196

	}

   197

   198

	/** Called by YAPF to calculate cost estimate. Calculates distance to the destination

   199

	 *  adds it to the actual cost from origin and stores the sum to the Node::m_estimate */

   200

	inline bool PfCalcEstimate(Node& n)

   201

	{

   202

		static int dg_dir_to_x_offs[] = {-1, 0, 1, 0};

   203

		static int dg_dir_to_y_offs[] = {0, 1, 0, -1};

   204

		if (PfDetectDestination(n)) {

   205

			n.m_estimate = n.m_cost;

   206

			return true;

   207

		}

   208

   209

		TileIndex tile = n.m_segment_last_tile;

   210

		DiagDirection exitdir = TrackdirToExitdir(n.m_segment_last_td);

   211

		int x1 = 2 * TileX(tile) + dg_dir_to_x_offs[(int)exitdir];

   212

		int y1 = 2 * TileY(tile) + dg_dir_to_y_offs[(int)exitdir];

   213

		int x2 = 2 * TileX(m_destTile);

   214

		int y2 = 2 * TileY(m_destTile);

   215

		int dx = abs(x1 - x2);

   216

		int dy = abs(y1 - y2);

   217

		int dmin = min(dx, dy);

   218

		int dxy = abs(dx - dy);

   219

		int d = dmin * YAPF_TILE_CORNER_LENGTH + (dxy - 1) * (YAPF_TILE_LENGTH / 2);

   220

		n.m_estimate = n.m_cost + d;

   221

		assert(n.m_estimate >= n.m_parent->m_estimate);

   222

		return true;

   223

	}

   224

};

   225

   226

   227

   228

template <class Types>

   229

class CYapfFollowRoadT

   230

{

   231

public:

   232

	typedef typename Types::Tpf Tpf;                     ///< the pathfinder class (derived from THIS class)

   233

	typedef typename Types::TrackFollower TrackFollower;

   234

	typedef typename Types::NodeList::Titem Node;        ///< this will be our node type

   235

	typedef typename Node::Key Key;                      ///< key to hash tables

   236

   237

protected:

   238

	/// to access inherited path finder

   239

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

   240

   241

public:

   242

   243

	/** Called by YAPF to move from the given node to the next tile. For each

   244

	 *  reachable trackdir on the new tile creates new node, initializes it

   245

	 *  and adds it to the open list by calling Yapf().AddNewNode(n) */

   246

	inline void PfFollowNode(Node& old_node)

   247

	{

   248

		TrackFollower F(Yapf().GetVehicle());

   249

		if (F.Follow(old_node.m_segment_last_tile, old_node.m_segment_last_td))

   250

			Yapf().AddMultipleNodes(&old_node, F);

   251

	}

   252

   253

	/// return debug report character to identify the transportation type

   254

	FORCEINLINE char TransportTypeChar() const {return 'r';}

   255

   256

	static Trackdir stChooseRoadTrack(const Vehicle *v, TileIndex tile, DiagDirection enterdir)

   257

	{

   258

		Tpf pf;

   259

		return pf.ChooseRoadTrack(v, tile, enterdir);

   260

	}

   261

   262

	FORCEINLINE Trackdir ChooseRoadTrack(const Vehicle *v, TileIndex tile, DiagDirection enterdir)

   263

	{

   264

		// handle special case - when next tile is destination tile

   265

		if (tile == v->dest_tile) {

   266

			// choose diagonal trackdir reachable from enterdir

   267

			return DiagDirToDiagTrackdir(enterdir);

   268

		}

   269

		// our source tile will be the next vehicle tile (should be the given one)

   270

		TileIndex src_tile = tile;

   271

		// get available trackdirs on the start tile

   272

		TrackdirBits src_trackdirs = TrackStatusToTrackdirBits(GetTileTrackStatus(tile, TRANSPORT_ROAD, v->u.road.compatible_roadtypes));

   273

		// select reachable trackdirs only

   274

		src_trackdirs &= DiagdirReachesTrackdirs(enterdir);

   275

   276

		// get available trackdirs on the destination tile

   277

		TileIndex dest_tile = v->dest_tile;

   278

		TrackdirBits dest_trackdirs = TrackStatusToTrackdirBits(GetTileTrackStatus(dest_tile, TRANSPORT_ROAD, v->u.road.compatible_roadtypes));

   279

   280

		// set origin and destination nodes

   281

		Yapf().SetOrigin(src_tile, src_trackdirs);

   282

		Yapf().SetDestination(dest_tile, dest_trackdirs);

   283

   284

		// find the best path

   285

		Yapf().FindPath(v);

   286

   287

		// if path not found - return INVALID_TRACKDIR

   288

		Trackdir next_trackdir = INVALID_TRACKDIR;

   289

		Node *pNode = Yapf().GetBestNode();

   290

		if (pNode != NULL) {

   291

			// path was found or at least suggested

   292

			// walk through the path back to its origin

   293

			while (pNode->m_parent != NULL) {

   294

				pNode = pNode->m_parent;

   295

			}

   296

			// return trackdir from the best origin node (one of start nodes)

   297

			Node& best_next_node = *pNode;

   298

			assert(best_next_node.GetTile() == tile);

   299

			next_trackdir = best_next_node.GetTrackdir();

   300

		}

   301

		return next_trackdir;

   302

	}

   303

   304

	static uint stDistanceToTile(const Vehicle *v, TileIndex tile)

   305

	{

   306

		Tpf pf;

   307

		return pf.DistanceToTile(v, tile);

   308

	}

   309

   310

	FORCEINLINE uint DistanceToTile(const Vehicle *v, TileIndex dst_tile)

   311

	{

   312

		// handle special case - when current tile is the destination tile

   313

		if (dst_tile == v->tile) {

   314

			// distance is zero in this case

   315

			return 0;

   316

		}

   317

   318

		if (!SetOriginFromVehiclePos(v)) return UINT_MAX;

   319

   320

		// set destination tile, trackdir

   321

		//   get available trackdirs on the destination tile

   322

		TrackdirBits dst_td_bits = TrackStatusToTrackdirBits(GetTileTrackStatus(dst_tile, TRANSPORT_ROAD, v->u.road.compatible_roadtypes));

   323

		Yapf().SetDestination(dst_tile, dst_td_bits);

   324

   325

		// if path not found - return distance = UINT_MAX

   326

		uint dist = UINT_MAX;

   327

   328

		// find the best path

   329

		if (!Yapf().FindPath(v)) return dist;

   330

   331

		Node *pNode = Yapf().GetBestNode();

   332

		if (pNode != NULL) {

   333

			// path was found

   334

			// get the path cost estimate

   335

			dist = pNode->GetCostEstimate();

   336

		}

   337

   338

		return dist;

   339

	}

   340

   341

	/** Return true if the valid origin (tile/trackdir) was set from the current vehicle position. */

   342

	FORCEINLINE bool SetOriginFromVehiclePos(const Vehicle *v)

   343

	{

   344

		// set origin (tile, trackdir)

   345

		TileIndex src_tile = v->tile;

   346

		Trackdir src_td = GetVehicleTrackdir(v);

   347

		if ((TrackStatusToTrackdirBits(GetTileTrackStatus(src_tile, TRANSPORT_ROAD, v->u.road.compatible_roadtypes)) & TrackdirToTrackdirBits(src_td)) == 0) {

   348

			// sometimes the roadveh is not on the road (it resides on non-existing track)

   349

			// how should we handle that situation?

   350

			return false;

   351

		}

   352

		Yapf().SetOrigin(src_tile, TrackdirToTrackdirBits(src_td));

   353

		return true;

   354

	}

   355

   356

	static Depot* stFindNearestDepot(const Vehicle* v, TileIndex tile, Trackdir td)

   357

	{

   358

		Tpf pf;

   359

		return pf.FindNearestDepot(v, tile, td);

   360

	}

   361

   362

	FORCEINLINE Depot* FindNearestDepot(const Vehicle* v, TileIndex tile, Trackdir td)

   363

	{

   364

		// set origin and destination nodes

   365

		Yapf().SetOrigin(tile, TrackdirToTrackdirBits(td));

   366

   367

		// find the best path

   368

		bool bFound = Yapf().FindPath(v);

   369

		if (!bFound) return false;

   370

   371

		// some path found

   372

		// get found depot tile

   373

		Node *n = Yapf().GetBestNode();

   374

		TileIndex depot_tile = n->m_segment_last_tile;

   375

		assert(IsRoadDepotTile(depot_tile));

   376

		Depot* ret = GetDepotByTile(depot_tile);

   377

		return ret;

   378

	}

   379

};

   380

   381

template <class Tpf_, class Tnode_list, template <class Types> class Tdestination>

   382

struct CYapfRoad_TypesT

   383

{

   384

	typedef CYapfRoad_TypesT<Tpf_, Tnode_list, Tdestination>  Types;

   385

   386

	typedef Tpf_                              Tpf;

   387

	typedef CFollowTrackRoad                  TrackFollower;

   388

	typedef Tnode_list                        NodeList;

   389

	typedef CYapfBaseT<Types>                 PfBase;

   390

	typedef CYapfFollowRoadT<Types>           PfFollow;

   391

	typedef CYapfOriginTileT<Types>           PfOrigin;

   392

	typedef Tdestination<Types>               PfDestination;

   393

	typedef CYapfSegmentCostCacheNoneT<Types> PfCache;

   394

	typedef CYapfCostRoadT<Types>             PfCost;

   395

};

   396

   397

struct CYapfRoad1         : CYapfT<CYapfRoad_TypesT<CYapfRoad1        , CRoadNodeListTrackDir, CYapfDestinationTileRoadT    > > {};

   398

struct CYapfRoad2         : CYapfT<CYapfRoad_TypesT<CYapfRoad2        , CRoadNodeListExitDir , CYapfDestinationTileRoadT    > > {};

   399

   400

struct CYapfRoadAnyDepot1 : CYapfT<CYapfRoad_TypesT<CYapfRoadAnyDepot1, CRoadNodeListTrackDir, CYapfDestinationAnyDepotRoadT> > {};

   401

struct CYapfRoadAnyDepot2 : CYapfT<CYapfRoad_TypesT<CYapfRoadAnyDepot2, CRoadNodeListExitDir , CYapfDestinationAnyDepotRoadT> > {};

   402

   403

   404

Trackdir YapfChooseRoadTrack(const Vehicle *v, TileIndex tile, DiagDirection enterdir)

   405

{

   406

	// default is YAPF type 2

   407

	typedef Trackdir (*PfnChooseRoadTrack)(const Vehicle*, TileIndex, DiagDirection);

   408

	PfnChooseRoadTrack pfnChooseRoadTrack = &CYapfRoad2::stChooseRoadTrack; // default: ExitDir, allow 90-deg

   409

   410

	// check if non-default YAPF type should be used

   411

	if (_settings_game.pf.yapf.disable_node_optimization) {

   412

		pfnChooseRoadTrack = &CYapfRoad1::stChooseRoadTrack; // Trackdir, allow 90-deg

   413

	}

   414

   415

	Trackdir td_ret = pfnChooseRoadTrack(v, tile, enterdir);

   416

	return td_ret;

   417

}

   418

   419

uint YapfRoadVehDistanceToTile(const Vehicle *v, TileIndex tile)

   420

{

   421

	// default is YAPF type 2

   422

	typedef uint (*PfnDistanceToTile)(const Vehicle*, TileIndex);

   423

	PfnDistanceToTile pfnDistanceToTile = &CYapfRoad2::stDistanceToTile; // default: ExitDir, allow 90-deg

   424

   425

	// check if non-default YAPF type should be used

   426

	if (_settings_game.pf.yapf.disable_node_optimization)

   427

		pfnDistanceToTile = &CYapfRoad1::stDistanceToTile; // Trackdir, allow 90-deg

   428

   429

	// measure distance in YAPF units

   430

	uint dist = pfnDistanceToTile(v, tile);

   431

	// convert distance to tiles

   432

	if (dist != UINT_MAX)

   433

		dist = (dist + YAPF_TILE_LENGTH - 1) / YAPF_TILE_LENGTH;

   434

	return dist;

   435

}

   436

   437

Depot* YapfFindNearestRoadDepot(const Vehicle *v)

   438

{

   439

	TileIndex tile = v->tile;

   440

	Trackdir trackdir = GetVehicleTrackdir(v);

   441

	if ((TrackStatusToTrackdirBits(GetTileTrackStatus(tile, TRANSPORT_ROAD, v->u.road.compatible_roadtypes)) & TrackdirToTrackdirBits(trackdir)) == 0)

   442

		return NULL;

   443

   444

	// handle the case when our vehicle is already in the depot tile

   445

	if (IsRoadDepotTile(tile)) {

   446

		// only what we need to return is the Depot*

   447

		return GetDepotByTile(tile);

   448

	}

   449

   450

	// default is YAPF type 2

   451

	typedef Depot* (*PfnFindNearestDepot)(const Vehicle*, TileIndex, Trackdir);

   452

	PfnFindNearestDepot pfnFindNearestDepot = &CYapfRoadAnyDepot2::stFindNearestDepot;

   453

   454

	// check if non-default YAPF type should be used

   455

	if (_settings_game.pf.yapf.disable_node_optimization)

   456

		pfnFindNearestDepot = &CYapfRoadAnyDepot1::stFindNearestDepot; // Trackdir, allow 90-deg

   457

   458

	Depot* ret = pfnFindNearestDepot(v, tile, trackdir);

   459

	return ret;

   460

}