1

/* $Id$ */

     2

     3

#include "../stdafx.h"

     4

     5

#include "yapf.hpp"

     6

#include "yapf_node_road.hpp"

     7

     8

     9

template <class Types>

    10

class CYapfCostRoadT

    11

{

    12

public:

    13

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

    14

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

    15

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

    16

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

    17

    18

protected:

    19

	/// to access inherited path finder

    20

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

    21

    22

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

    23

	{

    24

		// height of the center of the current tile

    25

		int x1 = TileX(tile) * TILE_SIZE;

    26

		int y1 = TileY(tile) * TILE_SIZE;

    27

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

    28

    29

		// height of the center of the next tile

    30

		int x2 = TileX(next_tile) * TILE_SIZE;

    31

		int y2 = TileY(next_tile) * TILE_SIZE;

    32

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

    33

    34

		if (z2 - z1 > 1) {

    35

			/* Slope up */

    36

			return Yapf().PfGetSettings().road_slope_penalty;

    37

		}

    38

		return 0;

    39

	}

    40

    41

	/** return one tile cost */

    42

	FORCEINLINE int OneTileCost(TileIndex tile, Trackdir trackdir)

    43

	{

    44

		int cost = 0;

    45

		// set base cost

    46

		if (IsDiagonalTrackdir(trackdir)) {

    47

			cost += YAPF_TILE_LENGTH;

    48

			switch (GetTileType(tile)) {

    49

				case MP_STREET:

    50

					/* Increase the cost for level crossings */

    51

					if (IsLevelCrossing(tile))

    52

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

    53

					break;

    54

    55

				default:

    56

					break;

    57

			}

    58

		} else {

    59

			// non-diagonal trackdir

    60

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

    61

		}

    62

		return cost;

    63

	}

    64

    65

public:

    66

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

    67

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

    68

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

    69

	FORCEINLINE bool PfCalcCost(Node& n)

    70

	{

    71

		int segment_cost = 0;

    72

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

    73

		TileIndex tile = n.m_key.m_tile;

    74

		Trackdir trackdir = n.m_key.m_td;

    75

		while (true) {

    76

			// base tile cost depending on distance between edges

    77

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

    78

    79

			// stop if we have just entered the depot

    80

			if (IsTileDepotType(tile, TRANSPORT_ROAD) && trackdir == DiagdirToDiagTrackdir(ReverseDiagDir(GetRoadDepotDirection(tile)))) {

    81

				// next time we will reverse and leave the depot

    82

				break;

    83

			}

    84

    85

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

    86

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

    87

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

    88

    89

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

    90

			if (KillFirstBit2x64(F.m_new_td_bits) != 0) break;

    91

    92

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

    93

    94

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

    95

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

    96

    97

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

    98

			segment_cost += F.m_tiles_skipped * YAPF_TILE_LENGTH;

    99

   100

			// add hilly terrain penalty

   101

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

   102

   103

			// add min/max speed penalties

   104

			int min_speed = 0;

   105

			int max_speed = F.GetSpeedLimit(&min_speed);

   106

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

   107

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

   108

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

   109

   110

			// move to the next tile

   111

			tile = F.m_new_tile;

   112

			trackdir = new_td;

   113

		};

   114

   115

		// save end of segment back to the node

   116

		n.m_segment_last_tile = tile;

   117

		n.m_segment_last_td = trackdir;

   118

   119

		// save also tile cost

   120

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

   121

		n.m_cost = parent_cost + segment_cost;

   122

		return true;

   123

	}

   124

};

   125

   126

   127

template <class Types>

   128

class CYapfDestinationAnyDepotRoadT

   129

{

   130

public:

   131

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

   132

	typedef typename Types::TrackFollower TrackFollower;

   133

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

   134

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

   135

   136

	/// to access inherited path finder

   137

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

   138

   139

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

   140

	FORCEINLINE bool PfDetectDestination(Node& n)

   141

	{

   142

		bool bDest = IsTileDepotType(n.m_segment_last_tile, TRANSPORT_ROAD);

   143

		return bDest;

   144

	}

   145

   146

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

   147

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

   148

	FORCEINLINE bool PfCalcEstimate(Node& n)

   149

	{

   150

		n.m_estimate = n.m_cost;

   151

		return true;

   152

	}

   153

};

   154

   155

   156

template <class Types>

   157

class CYapfDestinationTileRoadT

   158

{

   159

public:

   160

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

   161

	typedef typename Types::TrackFollower TrackFollower;

   162

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

   163

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

   164

   165

protected:

   166

	TileIndex    m_destTile;

   167

	TrackdirBits m_destTrackdirs;

   168

   169

public:

   170

	void SetDestination(TileIndex tile, TrackdirBits trackdirs)

   171

	{

   172

		m_destTile = tile;

   173

		m_destTrackdirs = trackdirs;

   174

	}

   175

   176

protected:

   177

	/// to access inherited path finder

   178

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

   179

   180

public:

   181

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

   182

	FORCEINLINE bool PfDetectDestination(Node& n)

   183

	{

   184

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

   185

		return bDest;

   186

	}

   187

   188

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

   189

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

   190

	inline bool PfCalcEstimate(Node& n)

   191

	{

   192

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

   193

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

   194

		if (PfDetectDestination(n)) {

   195

			n.m_estimate = n.m_cost;

   196

			return true;

   197

		}

   198

   199

		TileIndex tile = n.m_segment_last_tile;

   200

		DiagDirection exitdir = TrackdirToExitdir(n.m_segment_last_td);

   201

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

   202

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

   203

		int x2 = 2 * TileX(m_destTile);

   204

		int y2 = 2 * TileY(m_destTile);

   205

		int dx = abs(x1 - x2);

   206

		int dy = abs(y1 - y2);

   207

		int dmin = min(dx, dy);

   208

		int dxy = abs(dx - dy);

   209

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

   210

		n.m_estimate = n.m_cost + d;

   211

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

   212

		return true;

   213

	}

   214

};

   215

   216

   217

   218

template <class Types>

   219

class CYapfFollowRoadT

   220

{

   221

public:

   222

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

   223

	typedef typename Types::TrackFollower TrackFollower;

   224

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

   225

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

   226

   227

protected:

   228

	/// to access inherited path finder

   229

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

   230

   231

public:

   232

   233

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

   234

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

   235

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

   236

	inline void PfFollowNode(Node& old_node)

   237

	{

   238

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

   239

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

   240

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

   241

	}

   242

   243

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

   244

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

   245

   246

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

   247

	{

   248

		Tpf pf;

   249

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

   250

	}

   251

   252

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

   253

	{

   254

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

   255

		if (tile == v->dest_tile) {

   256

			// choose diagonal trackdir reachable from enterdir

   257

			return (Trackdir)DiagdirToDiagTrackdir(enterdir);

   258

		}

   259

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

   260

		TileIndex src_tile = tile;

   261

		// get available trackdirs on the start tile

   262

		uint ts = GetTileTrackStatus(tile, TRANSPORT_ROAD);

   263

		TrackdirBits src_trackdirs = (TrackdirBits)(ts & TRACKDIR_BIT_MASK);

   264

		// select reachable trackdirs only

   265

		src_trackdirs &= DiagdirReachesTrackdirs(enterdir);

   266

   267

		// get available trackdirs on the destination tile

   268

		TileIndex dest_tile = v->dest_tile;

   269

		uint dest_ts = GetTileTrackStatus(dest_tile, TRANSPORT_ROAD);

   270

		TrackdirBits dest_trackdirs = (TrackdirBits)(dest_ts & TRACKDIR_BIT_MASK);

   271

   272

		// set origin and destination nodes

   273

		Yapf().SetOrigin(src_tile, src_trackdirs);

   274

		Yapf().SetDestination(dest_tile, dest_trackdirs);

   275

   276

		// find the best path

   277

		Yapf().FindPath(v);

   278

   279

		// if path not found - return INVALID_TRACKDIR

   280

		Trackdir next_trackdir = INVALID_TRACKDIR;

   281

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

   282

		if (pNode != NULL) {

   283

			// path was found or at least suggested

   284

			// walk through the path back to its origin

   285

			while (pNode->m_parent != NULL) {

   286

				pNode = pNode->m_parent;

   287

			}

   288

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

   289

			Node& best_next_node = *pNode;

   290

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

   291

			next_trackdir = best_next_node.GetTrackdir();

   292

		}

   293

		return next_trackdir;

   294

	}

   295

   296

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

   297

	{

   298

		Tpf pf;

   299

		return pf.DistanceToTile(v, tile);

   300

	}

   301

   302

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

   303

	{

   304

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

   305

		if (dst_tile == v->tile) {

   306

			// distance is zero in this case

   307

			return 0;

   308

		}

   309

   310

		if (!SetOriginFromVehiclePos(v)) return UINT_MAX;

   311

   312

		// set destination tile, trackdir

   313

		//   get available trackdirs on the destination tile

   314

		uint dest_ts = GetTileTrackStatus(dst_tile, TRANSPORT_ROAD);

   315

		TrackdirBits dst_td_bits = (TrackdirBits)(dest_ts & TRACKDIR_BIT_MASK);

   316

		Yapf().SetDestination(dst_tile, dst_td_bits);

   317

   318

		// find the best path

   319

		Yapf().FindPath(v);

   320

   321

		// if path not found - return distance = UINT_MAX

   322

		uint dist = UINT_MAX;

   323

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

   324

		if (pNode != NULL) {

   325

			// path was found or at least suggested

   326

			// get the path cost estimate

   327

			dist = pNode->GetCostEstimate();

   328

		}

   329

   330

		return dist;

   331

	}

   332

   333

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

   334

	FORCEINLINE bool SetOriginFromVehiclePos(const Vehicle *v)

   335

	{

   336

		// set origin (tile, trackdir)

   337

		TileIndex src_tile = v->tile;

   338

		Trackdir src_td = GetVehicleTrackdir(v);

   339

		if ((GetTileTrackStatus(src_tile, TRANSPORT_ROAD) & TrackdirToTrackdirBits(src_td)) == 0) {

   340

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

   341

			// how should we handle that situation?

   342

			return false;

   343

		}

   344

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

   345

		return true;

   346

	}

   347

   348

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

   349

	{

   350

		Tpf pf;

   351

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

   352

	}

   353

   354

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

   355

	{

   356

		// set origin and destination nodes

   357

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

   358

   359

		// find the best path

   360

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

   361

		if (!bFound) return false;

   362

   363

		// some path found

   364

		// get found depot tile

   365

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

   366

		TileIndex depot_tile = n.m_segment_last_tile;

   367

		assert(IsTileDepotType(depot_tile, TRANSPORT_ROAD));

   368

		Depot* ret = GetDepotByTile(depot_tile);

   369

		return ret;

   370

	}

   371

};

   372

   373

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

   374

struct CYapfRoad_TypesT

   375

{

   376

	typedef CYapfRoad_TypesT<Tpf_, Tnode_list, Tdestination>  Types;

   377

   378

	typedef Tpf_                              Tpf;

   379

	typedef CFollowTrackRoad                  TrackFollower;

   380

	typedef Tnode_list                        NodeList;

   381

	typedef CYapfBaseT<Types>                 PfBase;

   382

	typedef CYapfFollowRoadT<Types>           PfFollow;

   383

	typedef CYapfOriginTileT<Types>           PfOrigin;

   384

	typedef Tdestination<Types>               PfDestination;

   385

	typedef CYapfSegmentCostCacheNoneT<Types> PfCache;

   386

	typedef CYapfCostRoadT<Types>             PfCost;

   387

};

   388

   389

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

   390

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

   391

   392

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

   393

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

   394

   395

   396

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

   397

{

   398

	// default is YAPF type 2

   399

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

   400

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

   401

   402

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

   403

	if (_patches.yapf.disable_node_optimization)

   404

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

   405

   406

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

   407

	return td_ret;

   408

}

   409

   410

uint YapfRoadVehDistanceToTile(const Vehicle* v, TileIndex tile)

   411

{

   412

	// default is YAPF type 2

   413

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

   414

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

   415

   416

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

   417

	if (_patches.yapf.disable_node_optimization)

   418

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

   419

   420

	// measure distance in YAPF units

   421

	uint dist = pfnDistanceToTile(v, tile);

   422

	// convert distance to tiles

   423

	if (dist != UINT_MAX)

   424

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

   425

	return dist;

   426

}

   427

   428

Depot* YapfFindNearestRoadDepot(const Vehicle *v)

   429

{

   430

	TileIndex tile = v->tile;

   431

	if (v->u.road.state == 255) tile = GetVehicleOutOfTunnelTile(v);

   432

	Trackdir trackdir = GetVehicleTrackdir(v);

   433

	if ((GetTileTrackStatus(tile, TRANSPORT_ROAD) & TrackdirToTrackdirBits(trackdir)) == 0)

   434

		return NULL;

   435

   436

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

   437

	if (IsTileType(tile, MP_STREET) && IsTileDepotType(tile, TRANSPORT_ROAD)) {

   438

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

   439

		return GetDepotByTile(tile);

   440

	}

   441

   442

	// default is YAPF type 2

   443

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

   444

	PfnFindNearestDepot pfnFindNearestDepot = &CYapfRoadAnyDepot2::stFindNearestDepot;

   445

   446

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

   447

	if (_patches.yapf.disable_node_optimization)

   448

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

   449

   450

	Depot* ret = pfnFindNearestDepot(const_cast<Vehicle*>(v), tile, trackdir);

   451

	return ret;

   452

}