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 = GetSlopeZ(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 = GetSlopeZ(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
00095 if (HasTrafficLights(tile)) cost += Yapf().PfGetSettings().road_trafficlight_penalty;
00096 }
00097 return cost;
00098 }
00099
00100 public:
00106 FORCEINLINE bool PfCalcCost(Node& n, const TrackFollower *tf)
00107 {
00108 int segment_cost = 0;
00109 uint tiles = 0;
00110
00111 TileIndex tile = n.m_key.m_tile;
00112 Trackdir trackdir = n.m_key.m_td;
00113 for (;;) {
00114
00115 segment_cost += Yapf().OneTileCost(tile, trackdir);
00116
00117 const RoadVehicle *v = Yapf().GetVehicle();
00118
00119 if (Yapf().PfDetectDestinationTile(tile, trackdir)) break;
00120
00121
00122 if (IsRoadDepotTile(tile) && trackdir == DiagDirToDiagTrackdir(ReverseDiagDir(GetRoadDepotDirection(tile)))) {
00123
00124 break;
00125 }
00126
00127
00128 TrackFollower F(Yapf().GetVehicle());
00129 if (!F.Follow(tile, trackdir)) break;
00130
00131
00132 if (KillFirstBit(F.m_new_td_bits) != TRACKDIR_BIT_NONE) break;
00133
00134 Trackdir new_td = (Trackdir)FindFirstBit2x64(F.m_new_td_bits);
00135
00136
00137 if (F.m_new_tile == n.m_key.m_tile && new_td == n.m_key.m_td) return false;
00138
00139
00140 segment_cost += F.m_tiles_skipped * YAPF_TILE_LENGTH;
00141 tiles += F.m_tiles_skipped + 1;
00142
00143
00144 segment_cost += Yapf().SlopeCost(tile, F.m_new_tile, trackdir);
00145
00146
00147 int min_speed = 0;
00148 int max_veh_speed = v->GetDisplayMaxSpeed();
00149 int max_speed = F.GetSpeedLimit(&min_speed);
00150 if (max_speed < max_veh_speed) segment_cost += 1 * (max_veh_speed - max_speed);
00151 if (min_speed > max_veh_speed) segment_cost += 10 * (min_speed - max_veh_speed);
00152
00153
00154 tile = F.m_new_tile;
00155 trackdir = new_td;
00156 if (tiles > MAX_MAP_SIZE) break;
00157 }
00158
00159
00160 n.m_segment_last_tile = tile;
00161 n.m_segment_last_td = trackdir;
00162
00163
00164 int parent_cost = (n.m_parent != NULL) ? n.m_parent->m_cost : 0;
00165 n.m_cost = parent_cost + segment_cost;
00166 return true;
00167 }
00168 };
00169
00170
00171 template <class Types>
00172 class CYapfDestinationAnyDepotRoadT
00173 {
00174 public:
00175 typedef typename Types::Tpf Tpf;
00176 typedef typename Types::TrackFollower TrackFollower;
00177 typedef typename Types::NodeList::Titem Node;
00178 typedef typename Node::Key Key;
00179
00181 Tpf& Yapf()
00182 {
00183 return *static_cast<Tpf*>(this);
00184 }
00185
00187 FORCEINLINE bool PfDetectDestination(Node& n)
00188 {
00189 bool bDest = IsRoadDepotTile(n.m_segment_last_tile);
00190 return bDest;
00191 }
00192
00193 FORCEINLINE bool PfDetectDestinationTile(TileIndex tile, Trackdir trackdir)
00194 {
00195 return IsRoadDepotTile(tile);
00196 }
00197
00202 FORCEINLINE bool PfCalcEstimate(Node& n)
00203 {
00204 n.m_estimate = n.m_cost;
00205 return true;
00206 }
00207 };
00208
00209
00210 template <class Types>
00211 class CYapfDestinationTileRoadT
00212 {
00213 public:
00214 typedef typename Types::Tpf Tpf;
00215 typedef typename Types::TrackFollower TrackFollower;
00216 typedef typename Types::NodeList::Titem Node;
00217 typedef typename Node::Key Key;
00218
00219 protected:
00220 TileIndex m_destTile;
00221 TrackdirBits m_destTrackdirs;
00222 StationID m_dest_station;
00223 bool m_bus;
00224 bool m_non_artic;
00225
00226 public:
00227 void SetDestination(const RoadVehicle *v)
00228 {
00229 if (v->current_order.IsType(OT_GOTO_STATION)) {
00230 m_dest_station = v->current_order.GetDestination();
00231 m_bus = v->IsBus();
00232 m_destTile = CalcClosestStationTile(m_dest_station, v->tile, m_bus ? STATION_BUS : STATION_TRUCK);
00233 m_non_artic = !v->HasArticulatedPart();
00234 m_destTrackdirs = INVALID_TRACKDIR_BIT;
00235 } else {
00236 m_dest_station = INVALID_STATION;
00237 m_destTile = v->dest_tile;
00238 m_destTrackdirs = TrackStatusToTrackdirBits(GetTileTrackStatus(v->dest_tile, TRANSPORT_ROAD, v->compatible_roadtypes));
00239 }
00240 }
00241
00242 protected:
00244 Tpf& Yapf()
00245 {
00246 return *static_cast<Tpf*>(this);
00247 }
00248
00249 public:
00251 FORCEINLINE bool PfDetectDestination(Node& n)
00252 {
00253 return PfDetectDestinationTile(n.m_segment_last_tile, n.m_segment_last_td);
00254 }
00255
00256 FORCEINLINE bool PfDetectDestinationTile(TileIndex tile, Trackdir trackdir)
00257 {
00258 if (m_dest_station != INVALID_STATION) {
00259 return IsTileType(tile, MP_STATION) &&
00260 GetStationIndex(tile) == m_dest_station &&
00261 (m_bus ? IsBusStop(tile) : IsTruckStop(tile)) &&
00262 (m_non_artic || IsDriveThroughStopTile(tile));
00263 }
00264
00265 return tile == m_destTile && ((m_destTrackdirs & TrackdirToTrackdirBits(trackdir)) != TRACKDIR_BIT_NONE);
00266 }
00267
00272 inline bool PfCalcEstimate(Node& n)
00273 {
00274 static const int dg_dir_to_x_offs[] = {-1, 0, 1, 0};
00275 static const int dg_dir_to_y_offs[] = {0, 1, 0, -1};
00276 if (PfDetectDestination(n)) {
00277 n.m_estimate = n.m_cost;
00278 return true;
00279 }
00280
00281 TileIndex tile = n.m_segment_last_tile;
00282 DiagDirection exitdir = TrackdirToExitdir(n.m_segment_last_td);
00283 int x1 = 2 * TileX(tile) + dg_dir_to_x_offs[(int)exitdir];
00284 int y1 = 2 * TileY(tile) + dg_dir_to_y_offs[(int)exitdir];
00285 int x2 = 2 * TileX(m_destTile);
00286 int y2 = 2 * TileY(m_destTile);
00287 int dx = abs(x1 - x2);
00288 int dy = abs(y1 - y2);
00289 int dmin = min(dx, dy);
00290 int dxy = abs(dx - dy);
00291 int d = dmin * YAPF_TILE_CORNER_LENGTH + (dxy - 1) * (YAPF_TILE_LENGTH / 2);
00292 n.m_estimate = n.m_cost + d;
00293 assert(n.m_estimate >= n.m_parent->m_estimate);
00294 return true;
00295 }
00296 };
00297
00298
00299
00300 template <class Types>
00301 class CYapfFollowRoadT
00302 {
00303 public:
00304 typedef typename Types::Tpf Tpf;
00305 typedef typename Types::TrackFollower TrackFollower;
00306 typedef typename Types::NodeList::Titem Node;
00307 typedef typename Node::Key Key;
00308
00309 protected:
00311 FORCEINLINE Tpf& Yapf()
00312 {
00313 return *static_cast<Tpf*>(this);
00314 }
00315
00316 public:
00317
00323 inline void PfFollowNode(Node& old_node)
00324 {
00325 TrackFollower F(Yapf().GetVehicle());
00326 if (F.Follow(old_node.m_segment_last_tile, old_node.m_segment_last_td)) {
00327 Yapf().AddMultipleNodes(&old_node, F);
00328 }
00329 }
00330
00332 FORCEINLINE char TransportTypeChar() const
00333 {
00334 return 'r';
00335 }
00336
00337 static Trackdir stChooseRoadTrack(const RoadVehicle *v, TileIndex tile, DiagDirection enterdir, bool &path_found)
00338 {
00339 Tpf pf;
00340 return pf.ChooseRoadTrack(v, tile, enterdir, path_found);
00341 }
00342
00343 FORCEINLINE Trackdir ChooseRoadTrack(const RoadVehicle *v, TileIndex tile, DiagDirection enterdir, bool &path_found)
00344 {
00345
00346
00347
00348
00349 if (tile == v->dest_tile && !v->current_order.IsType(OT_GOTO_STATION)) {
00350
00351 return DiagDirToDiagTrackdir(enterdir);
00352 }
00353
00354 TileIndex src_tile = tile;
00355
00356 TrackdirBits src_trackdirs = TrackStatusToTrackdirBits(GetTileTrackStatus(tile, TRANSPORT_ROAD, v->compatible_roadtypes));
00357
00358 src_trackdirs &= DiagdirReachesTrackdirs(enterdir);
00359
00360
00361 Yapf().SetOrigin(src_tile, src_trackdirs);
00362 Yapf().SetDestination(v);
00363
00364
00365 path_found = Yapf().FindPath(v);
00366
00367
00368 Trackdir next_trackdir = INVALID_TRACKDIR;
00369 Node *pNode = Yapf().GetBestNode();
00370 if (pNode != NULL) {
00371
00372
00373 while (pNode->m_parent != NULL) {
00374 pNode = pNode->m_parent;
00375 }
00376
00377 Node& best_next_node = *pNode;
00378 assert(best_next_node.GetTile() == tile);
00379 next_trackdir = best_next_node.GetTrackdir();
00380 }
00381 return next_trackdir;
00382 }
00383
00384 static uint stDistanceToTile(const RoadVehicle *v, TileIndex tile)
00385 {
00386 Tpf pf;
00387 return pf.DistanceToTile(v, tile);
00388 }
00389
00390 FORCEINLINE uint DistanceToTile(const RoadVehicle *v, TileIndex dst_tile)
00391 {
00392
00393 if (dst_tile == v->tile) {
00394
00395 return 0;
00396 }
00397
00398 if (!SetOriginFromVehiclePos(v)) return UINT_MAX;
00399
00400
00401 Yapf().SetDestination(v);
00402
00403
00404 uint dist = UINT_MAX;
00405
00406
00407 if (!Yapf().FindPath(v)) return dist;
00408
00409 Node *pNode = Yapf().GetBestNode();
00410 if (pNode != NULL) {
00411
00412
00413 dist = pNode->GetCostEstimate();
00414 }
00415
00416 return dist;
00417 }
00418
00420 FORCEINLINE bool SetOriginFromVehiclePos(const RoadVehicle *v)
00421 {
00422
00423 TileIndex src_tile = v->tile;
00424 Trackdir src_td = v->GetVehicleTrackdir();
00425 if ((TrackStatusToTrackdirBits(GetTileTrackStatus(src_tile, TRANSPORT_ROAD, v->compatible_roadtypes)) & TrackdirToTrackdirBits(src_td)) == 0) {
00426
00427
00428 return false;
00429 }
00430 Yapf().SetOrigin(src_tile, TrackdirToTrackdirBits(src_td));
00431 return true;
00432 }
00433
00434 static bool stFindNearestDepot(const RoadVehicle *v, TileIndex tile, Trackdir td, int max_distance, TileIndex *depot_tile)
00435 {
00436 Tpf pf;
00437 return pf.FindNearestDepot(v, tile, td, max_distance, depot_tile);
00438 }
00439
00440 FORCEINLINE bool FindNearestDepot(const RoadVehicle *v, TileIndex tile, Trackdir td, int max_distance, TileIndex *depot_tile)
00441 {
00442
00443 Yapf().SetOrigin(tile, TrackdirToTrackdirBits(td));
00444
00445
00446 bool bFound = Yapf().FindPath(v);
00447 if (!bFound) return false;
00448
00449
00450
00451 Node *n = Yapf().GetBestNode();
00452
00453 if (max_distance > 0 && n->m_cost > max_distance * YAPF_TILE_LENGTH) return false;
00454
00455 *depot_tile = n->m_segment_last_tile;
00456 return true;
00457 }
00458 };
00459
00460 template <class Tpf_, class Tnode_list, template <class Types> class Tdestination>
00461 struct CYapfRoad_TypesT
00462 {
00463 typedef CYapfRoad_TypesT<Tpf_, Tnode_list, Tdestination> Types;
00464
00465 typedef Tpf_ Tpf;
00466 typedef CFollowTrackRoad TrackFollower;
00467 typedef Tnode_list NodeList;
00468 typedef RoadVehicle VehicleType;
00469 typedef CYapfBaseT<Types> PfBase;
00470 typedef CYapfFollowRoadT<Types> PfFollow;
00471 typedef CYapfOriginTileT<Types> PfOrigin;
00472 typedef Tdestination<Types> PfDestination;
00473 typedef CYapfSegmentCostCacheNoneT<Types> PfCache;
00474 typedef CYapfCostRoadT<Types> PfCost;
00475 };
00476
00477 struct CYapfRoad1 : CYapfT<CYapfRoad_TypesT<CYapfRoad1 , CRoadNodeListTrackDir, CYapfDestinationTileRoadT > > {};
00478 struct CYapfRoad2 : CYapfT<CYapfRoad_TypesT<CYapfRoad2 , CRoadNodeListExitDir , CYapfDestinationTileRoadT > > {};
00479
00480 struct CYapfRoadAnyDepot1 : CYapfT<CYapfRoad_TypesT<CYapfRoadAnyDepot1, CRoadNodeListTrackDir, CYapfDestinationAnyDepotRoadT> > {};
00481 struct CYapfRoadAnyDepot2 : CYapfT<CYapfRoad_TypesT<CYapfRoadAnyDepot2, CRoadNodeListExitDir , CYapfDestinationAnyDepotRoadT> > {};
00482
00483
00484 Trackdir YapfRoadVehicleChooseTrack(const RoadVehicle *v, TileIndex tile, DiagDirection enterdir, TrackdirBits trackdirs, bool &path_found)
00485 {
00486
00487 typedef Trackdir (*PfnChooseRoadTrack)(const RoadVehicle*, TileIndex, DiagDirection, bool &path_found);
00488 PfnChooseRoadTrack pfnChooseRoadTrack = &CYapfRoad2::stChooseRoadTrack;
00489
00490
00491 if (_settings_game.pf.yapf.disable_node_optimization) {
00492 pfnChooseRoadTrack = &CYapfRoad1::stChooseRoadTrack;
00493 }
00494
00495 Trackdir td_ret = pfnChooseRoadTrack(v, tile, enterdir, path_found);
00496 return (td_ret != INVALID_TRACKDIR) ? td_ret : (Trackdir)FindFirstBit2x64(trackdirs);
00497 }
00498
00499 FindDepotData YapfRoadVehicleFindNearestDepot(const RoadVehicle *v, int max_distance)
00500 {
00501 TileIndex tile = v->tile;
00502 Trackdir trackdir = v->GetVehicleTrackdir();
00503 if ((TrackStatusToTrackdirBits(GetTileTrackStatus(tile, TRANSPORT_ROAD, v->compatible_roadtypes)) & TrackdirToTrackdirBits(trackdir)) == 0) {
00504 return FindDepotData();
00505 }
00506
00507
00508 typedef bool (*PfnFindNearestDepot)(const RoadVehicle*, TileIndex, Trackdir, int, TileIndex*);
00509 PfnFindNearestDepot pfnFindNearestDepot = &CYapfRoadAnyDepot2::stFindNearestDepot;
00510
00511
00512 if (_settings_game.pf.yapf.disable_node_optimization) {
00513 pfnFindNearestDepot = &CYapfRoadAnyDepot1::stFindNearestDepot;
00514 }
00515
00516 FindDepotData fdd;
00517 bool ret = pfnFindNearestDepot(v, tile, trackdir, max_distance, &fdd.tile);
00518 fdd.best_length = ret ? max_distance / 2 : UINT_MAX;
00519 return fdd;
00520 }