00001
00002
00003
00004
00005
00006
00007
00008
00009
00012 #include "../../stdafx.h"
00013 #include "yapf.hpp"
00014 #include "yapf_node_road.hpp"
00015 #include "../../roadstop_base.h"
00016
00017
00018 template <class Types>
00019 class CYapfCostRoadT
00020 {
00021 public:
00022 typedef typename Types::Tpf Tpf;
00023 typedef typename Types::TrackFollower TrackFollower;
00024 typedef typename Types::NodeList::Titem Node;
00025 typedef typename Node::Key Key;
00026
00027 protected:
00029 Tpf& Yapf()
00030 {
00031 return *static_cast<Tpf*>(this);
00032 }
00033
00034 int SlopeCost(TileIndex tile, TileIndex next_tile, Trackdir trackdir)
00035 {
00036
00037 int x1 = TileX(tile) * TILE_SIZE;
00038 int y1 = TileY(tile) * TILE_SIZE;
00039 int z1 = GetSlopePixelZ(x1 + TILE_SIZE / 2, y1 + TILE_SIZE / 2);
00040
00041
00042 int x2 = TileX(next_tile) * TILE_SIZE;
00043 int y2 = TileY(next_tile) * TILE_SIZE;
00044 int z2 = GetSlopePixelZ(x2 + TILE_SIZE / 2, y2 + TILE_SIZE / 2);
00045
00046 if (z2 - z1 > 1) {
00047
00048 return Yapf().PfGetSettings().road_slope_penalty;
00049 }
00050 return 0;
00051 }
00052
00054 FORCEINLINE int OneTileCost(TileIndex tile, Trackdir trackdir)
00055 {
00056 int cost = 0;
00057
00058 if (IsDiagonalTrackdir(trackdir)) {
00059 cost += YAPF_TILE_LENGTH;
00060 switch (GetTileType(tile)) {
00061 case MP_ROAD:
00062
00063 if (IsLevelCrossing(tile)) {
00064 cost += Yapf().PfGetSettings().road_crossing_penalty;
00065 }
00066 break;
00067
00068 case MP_STATION: {
00069 const RoadStop *rs = RoadStop::GetByTile(tile, GetRoadStopType(tile));
00070 if (IsDriveThroughStopTile(tile)) {
00071
00072 cost += Yapf().PfGetSettings().road_stop_penalty;
00073 DiagDirection dir = TrackdirToExitdir(trackdir);
00074 if (!RoadStop::IsDriveThroughRoadStopContinuation(tile, tile - TileOffsByDiagDir(dir))) {
00075
00076
00077 const RoadStop::Entry *entry = rs->GetEntry(dir);
00078 cost += entry->GetOccupied() * Yapf().PfGetSettings().road_stop_occupied_penalty / entry->GetLength();
00079 }
00080 } else {
00081
00082 cost += Yapf().PfGetSettings().road_stop_bay_occupied_penalty * (!rs->IsFreeBay(0) + !rs->IsFreeBay(1)) / 2;
00083 }
00084 break;
00085 }
00086
00087 default:
00088 break;
00089 }
00090 } else {
00091
00092 cost = YAPF_TILE_CORNER_LENGTH + Yapf().PfGetSettings().road_curve_penalty;
00093 }
00094 return cost;
00095 }
00096
00097 public:
00103 FORCEINLINE bool PfCalcCost(Node& n, const TrackFollower *tf)
00104 {
00105 int segment_cost = 0;
00106 uint tiles = 0;
00107
00108 TileIndex tile = n.m_key.m_tile;
00109 Trackdir trackdir = n.m_key.m_td;
00110 for (;;) {
00111
00112 segment_cost += Yapf().OneTileCost(tile, trackdir);
00113
00114 const RoadVehicle *v = Yapf().GetVehicle();
00115
00116 if (Yapf().PfDetectDestinationTile(tile, trackdir)) break;
00117
00118
00119 if (IsRoadDepotTile(tile) && trackdir == DiagDirToDiagTrackdir(ReverseDiagDir(GetRoadDepotDirection(tile)))) {
00120
00121 break;
00122 }
00123
00124
00125 TrackFollower F(Yapf().GetVehicle());
00126 if (!F.Follow(tile, trackdir)) break;
00127
00128
00129 if (KillFirstBit(F.m_new_td_bits) != TRACKDIR_BIT_NONE) break;
00130
00131 Trackdir new_td = (Trackdir)FindFirstBit2x64(F.m_new_td_bits);
00132
00133
00134 if (F.m_new_tile == n.m_key.m_tile && new_td == n.m_key.m_td) return false;
00135
00136
00137 segment_cost += F.m_tiles_skipped * YAPF_TILE_LENGTH;
00138 tiles += F.m_tiles_skipped + 1;
00139
00140
00141 segment_cost += Yapf().SlopeCost(tile, F.m_new_tile, trackdir);
00142
00143
00144 int min_speed = 0;
00145 int max_veh_speed = v->GetDisplayMaxSpeed();
00146 int max_speed = F.GetSpeedLimit(&min_speed);
00147 if (max_speed < max_veh_speed) segment_cost += 1 * (max_veh_speed - max_speed);
00148 if (min_speed > max_veh_speed) segment_cost += 10 * (min_speed - max_veh_speed);
00149
00150
00151 tile = F.m_new_tile;
00152 trackdir = new_td;
00153 if (tiles > MAX_MAP_SIZE) break;
00154 }
00155
00156
00157 n.m_segment_last_tile = tile;
00158 n.m_segment_last_td = trackdir;
00159
00160
00161 int parent_cost = (n.m_parent != NULL) ? n.m_parent->m_cost : 0;
00162 n.m_cost = parent_cost + segment_cost;
00163 return true;
00164 }
00165 };
00166
00167
00168 template <class Types>
00169 class CYapfDestinationAnyDepotRoadT
00170 {
00171 public:
00172 typedef typename Types::Tpf Tpf;
00173 typedef typename Types::TrackFollower TrackFollower;
00174 typedef typename Types::NodeList::Titem Node;
00175 typedef typename Node::Key Key;
00176
00178 Tpf& Yapf()
00179 {
00180 return *static_cast<Tpf*>(this);
00181 }
00182
00184 FORCEINLINE bool PfDetectDestination(Node& n)
00185 {
00186 bool bDest = IsRoadDepotTile(n.m_segment_last_tile);
00187 return bDest;
00188 }
00189
00190 FORCEINLINE bool PfDetectDestinationTile(TileIndex tile, Trackdir trackdir)
00191 {
00192 return IsRoadDepotTile(tile);
00193 }
00194
00199 FORCEINLINE bool PfCalcEstimate(Node& n)
00200 {
00201 n.m_estimate = n.m_cost;
00202 return true;
00203 }
00204 };
00205
00206
00207 template <class Types>
00208 class CYapfDestinationTileRoadT
00209 {
00210 public:
00211 typedef typename Types::Tpf Tpf;
00212 typedef typename Types::TrackFollower TrackFollower;
00213 typedef typename Types::NodeList::Titem Node;
00214 typedef typename Node::Key Key;
00215
00216 protected:
00217 TileIndex m_destTile;
00218 TrackdirBits m_destTrackdirs;
00219 StationID m_dest_station;
00220 bool m_bus;
00221 bool m_non_artic;
00222
00223 public:
00224 void SetDestination(const RoadVehicle *v)
00225 {
00226 if (v->current_order.IsType(OT_GOTO_STATION)) {
00227 m_dest_station = v->current_order.GetDestination();
00228 m_bus = v->IsBus();
00229 m_destTile = CalcClosestStationTile(m_dest_station, v->tile, m_bus ? STATION_BUS : STATION_TRUCK);
00230 m_non_artic = !v->HasArticulatedPart();
00231 m_destTrackdirs = INVALID_TRACKDIR_BIT;
00232 } else {
00233 m_dest_station = INVALID_STATION;
00234 m_destTile = v->dest_tile;
00235 m_destTrackdirs = TrackStatusToTrackdirBits(GetTileTrackStatus(v->dest_tile, TRANSPORT_ROAD, v->compatible_roadtypes));
00236 }
00237 }
00238
00239 protected:
00241 Tpf& Yapf()
00242 {
00243 return *static_cast<Tpf*>(this);
00244 }
00245
00246 public:
00248 FORCEINLINE bool PfDetectDestination(Node& n)
00249 {
00250 return PfDetectDestinationTile(n.m_segment_last_tile, n.m_segment_last_td);
00251 }
00252
00253 FORCEINLINE bool PfDetectDestinationTile(TileIndex tile, Trackdir trackdir)
00254 {
00255 if (m_dest_station != INVALID_STATION) {
00256 return IsTileType(tile, MP_STATION) &&
00257 GetStationIndex(tile) == m_dest_station &&
00258 (m_bus ? IsBusStop(tile) : IsTruckStop(tile)) &&
00259 (m_non_artic || IsDriveThroughStopTile(tile));
00260 }
00261
00262 return tile == m_destTile && ((m_destTrackdirs & TrackdirToTrackdirBits(trackdir)) != TRACKDIR_BIT_NONE);
00263 }
00264
00269 inline bool PfCalcEstimate(Node& n)
00270 {
00271 static const int dg_dir_to_x_offs[] = {-1, 0, 1, 0};
00272 static const int dg_dir_to_y_offs[] = {0, 1, 0, -1};
00273 if (PfDetectDestination(n)) {
00274 n.m_estimate = n.m_cost;
00275 return true;
00276 }
00277
00278 TileIndex tile = n.m_segment_last_tile;
00279 DiagDirection exitdir = TrackdirToExitdir(n.m_segment_last_td);
00280 int x1 = 2 * TileX(tile) + dg_dir_to_x_offs[(int)exitdir];
00281 int y1 = 2 * TileY(tile) + dg_dir_to_y_offs[(int)exitdir];
00282 int x2 = 2 * TileX(m_destTile);
00283 int y2 = 2 * TileY(m_destTile);
00284 int dx = abs(x1 - x2);
00285 int dy = abs(y1 - y2);
00286 int dmin = min(dx, dy);
00287 int dxy = abs(dx - dy);
00288 int d = dmin * YAPF_TILE_CORNER_LENGTH + (dxy - 1) * (YAPF_TILE_LENGTH / 2);
00289 n.m_estimate = n.m_cost + d;
00290 assert(n.m_estimate >= n.m_parent->m_estimate);
00291 return true;
00292 }
00293 };
00294
00295
00296
00297 template <class Types>
00298 class CYapfFollowRoadT
00299 {
00300 public:
00301 typedef typename Types::Tpf Tpf;
00302 typedef typename Types::TrackFollower TrackFollower;
00303 typedef typename Types::NodeList::Titem Node;
00304 typedef typename Node::Key Key;
00305
00306 protected:
00308 FORCEINLINE Tpf& Yapf()
00309 {
00310 return *static_cast<Tpf*>(this);
00311 }
00312
00313 public:
00314
00320 inline void PfFollowNode(Node& old_node)
00321 {
00322 TrackFollower F(Yapf().GetVehicle());
00323 if (F.Follow(old_node.m_segment_last_tile, old_node.m_segment_last_td)) {
00324 Yapf().AddMultipleNodes(&old_node, F);
00325 }
00326 }
00327
00329 FORCEINLINE char TransportTypeChar() const
00330 {
00331 return 'r';
00332 }
00333
00334 static Trackdir stChooseRoadTrack(const RoadVehicle *v, TileIndex tile, DiagDirection enterdir, bool &path_found)
00335 {
00336 Tpf pf;
00337 return pf.ChooseRoadTrack(v, tile, enterdir, path_found);
00338 }
00339
00340 FORCEINLINE Trackdir ChooseRoadTrack(const RoadVehicle *v, TileIndex tile, DiagDirection enterdir, bool &path_found)
00341 {
00342
00343
00344
00345
00346 if (tile == v->dest_tile && !v->current_order.IsType(OT_GOTO_STATION)) {
00347
00348 return DiagDirToDiagTrackdir(enterdir);
00349 }
00350
00351 TileIndex src_tile = tile;
00352
00353 TrackdirBits src_trackdirs = TrackStatusToTrackdirBits(GetTileTrackStatus(tile, TRANSPORT_ROAD, v->compatible_roadtypes));
00354
00355 src_trackdirs &= DiagdirReachesTrackdirs(enterdir);
00356
00357
00358 Yapf().SetOrigin(src_tile, src_trackdirs);
00359 Yapf().SetDestination(v);
00360
00361
00362 path_found = Yapf().FindPath(v);
00363
00364
00365 Trackdir next_trackdir = INVALID_TRACKDIR;
00366 Node *pNode = Yapf().GetBestNode();
00367 if (pNode != NULL) {
00368
00369
00370 while (pNode->m_parent != NULL) {
00371 pNode = pNode->m_parent;
00372 }
00373
00374 Node& best_next_node = *pNode;
00375 assert(best_next_node.GetTile() == tile);
00376 next_trackdir = best_next_node.GetTrackdir();
00377 }
00378 return next_trackdir;
00379 }
00380
00381 static uint stDistanceToTile(const RoadVehicle *v, TileIndex tile)
00382 {
00383 Tpf pf;
00384 return pf.DistanceToTile(v, tile);
00385 }
00386
00387 FORCEINLINE uint DistanceToTile(const RoadVehicle *v, TileIndex dst_tile)
00388 {
00389
00390 if (dst_tile == v->tile) {
00391
00392 return 0;
00393 }
00394
00395 if (!SetOriginFromVehiclePos(v)) return UINT_MAX;
00396
00397
00398 Yapf().SetDestination(v);
00399
00400
00401 uint dist = UINT_MAX;
00402
00403
00404 if (!Yapf().FindPath(v)) return dist;
00405
00406 Node *pNode = Yapf().GetBestNode();
00407 if (pNode != NULL) {
00408
00409
00410 dist = pNode->GetCostEstimate();
00411 }
00412
00413 return dist;
00414 }
00415
00417 FORCEINLINE bool SetOriginFromVehiclePos(const RoadVehicle *v)
00418 {
00419
00420 TileIndex src_tile = v->tile;
00421 Trackdir src_td = v->GetVehicleTrackdir();
00422 if ((TrackStatusToTrackdirBits(GetTileTrackStatus(src_tile, TRANSPORT_ROAD, v->compatible_roadtypes)) & TrackdirToTrackdirBits(src_td)) == 0) {
00423
00424
00425 return false;
00426 }
00427 Yapf().SetOrigin(src_tile, TrackdirToTrackdirBits(src_td));
00428 return true;
00429 }
00430
00431 static bool stFindNearestDepot(const RoadVehicle *v, TileIndex tile, Trackdir td, int max_distance, TileIndex *depot_tile)
00432 {
00433 Tpf pf;
00434 return pf.FindNearestDepot(v, tile, td, max_distance, depot_tile);
00435 }
00436
00437 FORCEINLINE bool FindNearestDepot(const RoadVehicle *v, TileIndex tile, Trackdir td, int max_distance, TileIndex *depot_tile)
00438 {
00439
00440 Yapf().SetOrigin(tile, TrackdirToTrackdirBits(td));
00441
00442
00443 bool bFound = Yapf().FindPath(v);
00444 if (!bFound) return false;
00445
00446
00447
00448 Node *n = Yapf().GetBestNode();
00449
00450 if (max_distance > 0 && n->m_cost > max_distance * YAPF_TILE_LENGTH) return false;
00451
00452 *depot_tile = n->m_segment_last_tile;
00453 return true;
00454 }
00455 };
00456
00457 template <class Tpf_, class Tnode_list, template <class Types> class Tdestination>
00458 struct CYapfRoad_TypesT
00459 {
00460 typedef CYapfRoad_TypesT<Tpf_, Tnode_list, Tdestination> Types;
00461
00462 typedef Tpf_ Tpf;
00463 typedef CFollowTrackRoad TrackFollower;
00464 typedef Tnode_list NodeList;
00465 typedef RoadVehicle VehicleType;
00466 typedef CYapfBaseT<Types> PfBase;
00467 typedef CYapfFollowRoadT<Types> PfFollow;
00468 typedef CYapfOriginTileT<Types> PfOrigin;
00469 typedef Tdestination<Types> PfDestination;
00470 typedef CYapfSegmentCostCacheNoneT<Types> PfCache;
00471 typedef CYapfCostRoadT<Types> PfCost;
00472 };
00473
00474 struct CYapfRoad1 : CYapfT<CYapfRoad_TypesT<CYapfRoad1 , CRoadNodeListTrackDir, CYapfDestinationTileRoadT > > {};
00475 struct CYapfRoad2 : CYapfT<CYapfRoad_TypesT<CYapfRoad2 , CRoadNodeListExitDir , CYapfDestinationTileRoadT > > {};
00476
00477 struct CYapfRoadAnyDepot1 : CYapfT<CYapfRoad_TypesT<CYapfRoadAnyDepot1, CRoadNodeListTrackDir, CYapfDestinationAnyDepotRoadT> > {};
00478 struct CYapfRoadAnyDepot2 : CYapfT<CYapfRoad_TypesT<CYapfRoadAnyDepot2, CRoadNodeListExitDir , CYapfDestinationAnyDepotRoadT> > {};
00479
00480
00481 Trackdir YapfRoadVehicleChooseTrack(const RoadVehicle *v, TileIndex tile, DiagDirection enterdir, TrackdirBits trackdirs, bool &path_found)
00482 {
00483
00484 typedef Trackdir (*PfnChooseRoadTrack)(const RoadVehicle*, TileIndex, DiagDirection, bool &path_found);
00485 PfnChooseRoadTrack pfnChooseRoadTrack = &CYapfRoad2::stChooseRoadTrack;
00486
00487
00488 if (_settings_game.pf.yapf.disable_node_optimization) {
00489 pfnChooseRoadTrack = &CYapfRoad1::stChooseRoadTrack;
00490 }
00491
00492 Trackdir td_ret = pfnChooseRoadTrack(v, tile, enterdir, path_found);
00493 return (td_ret != INVALID_TRACKDIR) ? td_ret : (Trackdir)FindFirstBit2x64(trackdirs);
00494 }
00495
00496 FindDepotData YapfRoadVehicleFindNearestDepot(const RoadVehicle *v, int max_distance)
00497 {
00498 TileIndex tile = v->tile;
00499 Trackdir trackdir = v->GetVehicleTrackdir();
00500 if ((TrackStatusToTrackdirBits(GetTileTrackStatus(tile, TRANSPORT_ROAD, v->compatible_roadtypes)) & TrackdirToTrackdirBits(trackdir)) == 0) {
00501 return FindDepotData();
00502 }
00503
00504
00505 typedef bool (*PfnFindNearestDepot)(const RoadVehicle*, TileIndex, Trackdir, int, TileIndex*);
00506 PfnFindNearestDepot pfnFindNearestDepot = &CYapfRoadAnyDepot2::stFindNearestDepot;
00507
00508
00509 if (_settings_game.pf.yapf.disable_node_optimization) {
00510 pfnFindNearestDepot = &CYapfRoadAnyDepot1::stFindNearestDepot;
00511 }
00512
00513 FindDepotData fdd;
00514 bool ret = pfnFindNearestDepot(v, tile, trackdir, max_distance, &fdd.tile);
00515 fdd.best_length = ret ? max_distance / 2 : UINT_MAX;
00516 return fdd;
00517 }