# yapf_road.cpp

Go to the documentation of this file.

    00001 /* $Id: yapf_road.cpp 15609 2009-03-04 08:02:16Z peter1138 $ */
    00002
    00005 #include "../stdafx.h"
    00006 #include "../depot_base.h"
    00007
    00008 #include "yapf.hpp"
    00009 #include "yapf_node_road.hpp"
    00010
    00011
    00012 template <class Types>
    00013 class CYapfCostRoadT
    00014 {
    00015 public:
    00016   typedef typename Types::Tpf Tpf;
    00017   typedef typename Types::TrackFollower TrackFollower;
    00018   typedef typename Types::NodeList::Titem Node;
    00019   typedef typename Node::Key Key;
    00020
    00021 protected:
    00023   Tpf& Yapf()
    00024   {
    00025     return *static_cast<Tpf*>(this);
    00026   }
    00027
    00028   int SlopeCost(TileIndex tile, TileIndex next_tile, Trackdir trackdir)
    00029   {
    00030     // height of the center of the current tile
    00031     int x1 = TileX(tile) * TILE_SIZE;
    00032     int y1 = TileY(tile) * TILE_SIZE;
    00033     int z1 = GetSlopeZ(x1 + TILE_SIZE / 2, y1 + TILE_SIZE / 2);
    00034
    00035     // height of the center of the next tile
    00036     int x2 = TileX(next_tile) * TILE_SIZE;
    00037     int y2 = TileY(next_tile) * TILE_SIZE;
    00038     int z2 = GetSlopeZ(x2 + TILE_SIZE / 2, y2 + TILE_SIZE / 2);
    00039
    00040     if (z2 - z1 > 1) {
    00041       /* Slope up */
    00042       return Yapf().PfGetSettings().road_slope_penalty;
    00043     }
    00044     return 0;
    00045   }
    00046
    00048   FORCEINLINE int OneTileCost(TileIndex tile, Trackdir trackdir)
    00049   {
    00050     int cost = 0;
    00051     // set base cost
    00052     if (IsDiagonalTrackdir(trackdir)) {
    00053       cost += YAPF_TILE_LENGTH;
    00054       switch (GetTileType(tile)) {
    00055         case MP_ROAD:
    00056           /* Increase the cost for level crossings */
    00057           if (IsLevelCrossing(tile)) {
    00058             cost += Yapf().PfGetSettings().road_crossing_penalty;
    00059           }
    00060           break;
    00061
    00062         case MP_STATION:
    00063           if (IsDriveThroughStopTile(tile)) {
    00064             cost += Yapf().PfGetSettings().road_stop_penalty;
    00065           }
    00066           break;
    00067
    00068         default:
    00069           break;
    00070       }
    00071     } else {
    00072       // non-diagonal trackdir
    00073       cost = YAPF_TILE_CORNER_LENGTH + Yapf().PfGetSettings().road_curve_penalty;
    00074     }
    00075     return cost;
    00076   }
    00077
    00078 public:
    00082   FORCEINLINE bool PfCalcCost(Node& n, const TrackFollower *tf)
    00083   {
    00084     int segment_cost = 0;
    00085     // start at n.m_key.m_tile / n.m_key.m_td and walk to the end of segment
    00086     TileIndex tile = n.m_key.m_tile;
    00087     Trackdir trackdir = n.m_key.m_td;
    00088     while (true) {
    00089       // base tile cost depending on distance between edges
    00090       segment_cost += Yapf().OneTileCost(tile, trackdir);
    00091
    00092       const Vehicle *v = Yapf().GetVehicle();
    00093       // we have reached the vehicle's destination - segment should end here to avoid target skipping
    00094       if (Yapf().PfDetectDestinationTile(tile, trackdir)) break;
    00095
    00096       // stop if we have just entered the depot
    00097       if (IsRoadDepotTile(tile) && trackdir == DiagDirToDiagTrackdir(ReverseDiagDir(GetRoadDepotDirection(tile)))) {
    00098         // next time we will reverse and leave the depot
    00099         break;
    00100       }
    00101
    00102       // if there are no reachable trackdirs on new tile, we have end of road
    00103       TrackFollower F(Yapf().GetVehicle());
    00104       if (!F.Follow(tile, trackdir)) break;
    00105
    00106       // if there are more trackdirs available & reachable, we are at the end of segment
    00107       if (KillFirstBit(F.m_new_td_bits) != TRACKDIR_BIT_NONE) break;
    00108
    00109       Trackdir new_td = (Trackdir)FindFirstBit2x64(F.m_new_td_bits);
    00110
    00111       // stop if RV is on simple loop with no junctions
    00112       if (F.m_new_tile == n.m_key.m_tile && new_td == n.m_key.m_td) return false;
    00113
    00114       // if we skipped some tunnel tiles, add their cost
    00115       segment_cost += F.m_tiles_skipped * YAPF_TILE_LENGTH;
    00116
    00117       // add hilly terrain penalty
    00118       segment_cost += Yapf().SlopeCost(tile, F.m_new_tile, trackdir);
    00119
    00120       // add min/max speed penalties
    00121       int min_speed = 0;
    00122       int max_speed = F.GetSpeedLimit(&min_speed);
    00123       if (max_speed < v->max_speed) segment_cost += 1 * (v->max_speed - max_speed);
    00124       if (min_speed > v->max_speed) segment_cost += 10 * (min_speed - v->max_speed);
    00125
    00126       // move to the next tile
    00127       tile = F.m_new_tile;
    00128       trackdir = new_td;
    00129     };
    00130
    00131     // save end of segment back to the node
    00132     n.m_segment_last_tile = tile;
    00133     n.m_segment_last_td = trackdir;
    00134
    00135     // save also tile cost
    00136     int parent_cost = (n.m_parent != NULL) ? n.m_parent->m_cost : 0;
    00137     n.m_cost = parent_cost + segment_cost;
    00138     return true;
    00139   }
    00140 };
    00141
    00142
    00143 template <class Types>
    00144 class CYapfDestinationAnyDepotRoadT
    00145 {
    00146 public:
    00147   typedef typename Types::Tpf Tpf;
    00148   typedef typename Types::TrackFollower TrackFollower;
    00149   typedef typename Types::NodeList::Titem Node;
    00150   typedef typename Node::Key Key;
    00151
    00153   Tpf& Yapf()
    00154   {
    00155     return *static_cast<Tpf*>(this);
    00156   }
    00157
    00159   FORCEINLINE bool PfDetectDestination(Node& n)
    00160   {
    00161     bool bDest = IsRoadDepotTile(n.m_segment_last_tile);
    00162     return bDest;
    00163   }
    00164
    00165   FORCEINLINE bool PfDetectDestinationTile(TileIndex tile, Trackdir trackdir)
    00166   {
    00167     return IsRoadDepotTile(tile);
    00168   }
    00169
    00172   FORCEINLINE bool PfCalcEstimate(Node& n)
    00173   {
    00174     n.m_estimate = n.m_cost;
    00175     return true;
    00176   }
    00177 };
    00178
    00179
    00180 template <class Types>
    00181 class CYapfDestinationTileRoadT
    00182 {
    00183 public:
    00184   typedef typename Types::Tpf Tpf;
    00185   typedef typename Types::TrackFollower TrackFollower;
    00186   typedef typename Types::NodeList::Titem Node;
    00187   typedef typename Node::Key Key;
    00188
    00189 protected:
    00190   TileIndex    m_destTile;
    00191   TrackdirBits m_destTrackdirs;
    00192
    00193 public:
    00194   void SetDestination(TileIndex tile, TrackdirBits trackdirs)
    00195   {
    00196     m_destTile = tile;
    00197     m_destTrackdirs = trackdirs;
    00198   }
    00199
    00200 protected:
    00202   Tpf& Yapf()
    00203   {
    00204     return *static_cast<Tpf*>(this);
    00205   }
    00206
    00207 public:
    00209   FORCEINLINE bool PfDetectDestination(Node& n)
    00210   {
    00211     bool bDest = (n.m_segment_last_tile == m_destTile) && ((m_destTrackdirs & TrackdirToTrackdirBits(n.m_segment_last_td)) != TRACKDIR_BIT_NONE);
    00212     return bDest;
    00213   }
    00214
    00215   FORCEINLINE bool PfDetectDestinationTile(TileIndex tile, Trackdir trackdir)
    00216   {
    00217     return tile == m_destTile && ((m_destTrackdirs & TrackdirToTrackdirBits(trackdir)) != TRACKDIR_BIT_NONE);
    00218   }
    00219
    00222   inline bool PfCalcEstimate(Node& n)
    00223   {
    00224     static int dg_dir_to_x_offs[] = {-1, 0, 1, 0};
    00225     static int dg_dir_to_y_offs[] = {0, 1, 0, -1};
    00226     if (PfDetectDestination(n)) {
    00227       n.m_estimate = n.m_cost;
    00228       return true;
    00229     }
    00230
    00231     TileIndex tile = n.m_segment_last_tile;
    00232     DiagDirection exitdir = TrackdirToExitdir(n.m_segment_last_td);
    00233     int x1 = 2 * TileX(tile) + dg_dir_to_x_offs[(int)exitdir];
    00234     int y1 = 2 * TileY(tile) + dg_dir_to_y_offs[(int)exitdir];
    00235     int x2 = 2 * TileX(m_destTile);
    00236     int y2 = 2 * TileY(m_destTile);
    00237     int dx = abs(x1 - x2);
    00238     int dy = abs(y1 - y2);
    00239     int dmin = min(dx, dy);
    00240     int dxy = abs(dx - dy);
    00241     int d = dmin * YAPF_TILE_CORNER_LENGTH + (dxy - 1) * (YAPF_TILE_LENGTH / 2);
    00242     n.m_estimate = n.m_cost + d;
    00243     assert(n.m_estimate >= n.m_parent->m_estimate);
    00244     return true;
    00245   }
    00246 };
    00247
    00248
    00249
    00250 template <class Types>
    00251 class CYapfFollowRoadT
    00252 {
    00253 public:
    00254   typedef typename Types::Tpf Tpf;
    00255   typedef typename Types::TrackFollower TrackFollower;
    00256   typedef typename Types::NodeList::Titem Node;
    00257   typedef typename Node::Key Key;
    00258
    00259 protected:
    00261   FORCEINLINE Tpf& Yapf()
    00262   {
    00263     return *static_cast<Tpf*>(this);
    00264   }
    00265
    00266 public:
    00267
    00271   inline void PfFollowNode(Node& old_node)
    00272   {
    00273     TrackFollower F(Yapf().GetVehicle());
    00274     if (F.Follow(old_node.m_segment_last_tile, old_node.m_segment_last_td)) {
    00275       Yapf().AddMultipleNodes(&old_node, F);
    00276     }
    00277   }
    00278
    00280   FORCEINLINE char TransportTypeChar() const
    00281   {
    00282     return 'r';
    00283   }
    00284
    00285   static Trackdir stChooseRoadTrack(const Vehicle *v, TileIndex tile, DiagDirection enterdir)
    00286   {
    00287     Tpf pf;
    00288     return pf.ChooseRoadTrack(v, tile, enterdir);
    00289   }
    00290
    00291   FORCEINLINE Trackdir ChooseRoadTrack(const Vehicle *v, TileIndex tile, DiagDirection enterdir)
    00292   {
    00293     // handle special case - when next tile is destination tile
    00294     if (tile == v->dest_tile) {
    00295       // choose diagonal trackdir reachable from enterdir
    00296       return DiagDirToDiagTrackdir(enterdir);
    00297     }
    00298     // our source tile will be the next vehicle tile (should be the given one)
    00299     TileIndex src_tile = tile;
    00300     // get available trackdirs on the start tile
    00301     TrackdirBits src_trackdirs = TrackStatusToTrackdirBits(GetTileTrackStatus(tile, TRANSPORT_ROAD, v->u.road.compatible_roadtypes));
    00302     // select reachable trackdirs only
    00303     src_trackdirs &= DiagdirReachesTrackdirs(enterdir);
    00304
    00305     // get available trackdirs on the destination tile
    00306     TileIndex dest_tile = v->dest_tile;
    00307     TrackdirBits dest_trackdirs = TrackStatusToTrackdirBits(GetTileTrackStatus(dest_tile, TRANSPORT_ROAD, v->u.road.compatible_roadtypes));
    00308
    00309     // set origin and destination nodes
    00310     Yapf().SetOrigin(src_tile, src_trackdirs);
    00311     Yapf().SetDestination(dest_tile, dest_trackdirs);
    00312
    00313     // find the best path
    00314     Yapf().FindPath(v);
    00315
    00316     // if path not found - return INVALID_TRACKDIR
    00317     Trackdir next_trackdir = INVALID_TRACKDIR;
    00318     Node *pNode = Yapf().GetBestNode();
    00319     if (pNode != NULL) {
    00320       // path was found or at least suggested
    00321       // walk through the path back to its origin
    00322       while (pNode->m_parent != NULL) {
    00323         pNode = pNode->m_parent;
    00324       }
    00325       // return trackdir from the best origin node (one of start nodes)
    00326       Node& best_next_node = *pNode;
    00327       assert(best_next_node.GetTile() == tile);
    00328       next_trackdir = best_next_node.GetTrackdir();
    00329     }
    00330     return next_trackdir;
    00331   }
    00332
    00333   static uint stDistanceToTile(const Vehicle *v, TileIndex tile)
    00334   {
    00335     Tpf pf;
    00336     return pf.DistanceToTile(v, tile);
    00337   }
    00338
    00339   FORCEINLINE uint DistanceToTile(const Vehicle *v, TileIndex dst_tile)
    00340   {
    00341     // handle special case - when current tile is the destination tile
    00342     if (dst_tile == v->tile) {
    00343       // distance is zero in this case
    00344       return 0;
    00345     }
    00346
    00347     if (!SetOriginFromVehiclePos(v)) return UINT_MAX;
    00348
    00349     // set destination tile, trackdir
    00350     //   get available trackdirs on the destination tile
    00351     TrackdirBits dst_td_bits = TrackStatusToTrackdirBits(GetTileTrackStatus(dst_tile, TRANSPORT_ROAD, v->u.road.compatible_roadtypes));
    00352     Yapf().SetDestination(dst_tile, dst_td_bits);
    00353
    00354     // if path not found - return distance = UINT_MAX
    00355     uint dist = UINT_MAX;
    00356
    00357     // find the best path
    00358     if (!Yapf().FindPath(v)) return dist;
    00359
    00360     Node *pNode = Yapf().GetBestNode();
    00361     if (pNode != NULL) {
    00362       // path was found
    00363       // get the path cost estimate
    00364       dist = pNode->GetCostEstimate();
    00365     }
    00366
    00367     return dist;
    00368   }
    00369
    00371   FORCEINLINE bool SetOriginFromVehiclePos(const Vehicle *v)
    00372   {
    00373     // set origin (tile, trackdir)
    00374     TileIndex src_tile = v->tile;
    00375     Trackdir src_td = GetVehicleTrackdir(v);
    00376     if ((TrackStatusToTrackdirBits(GetTileTrackStatus(src_tile, TRANSPORT_ROAD, v->u.road.compatible_roadtypes)) & TrackdirToTrackdirBits(src_td)) == 0) {
    00377       // sometimes the roadveh is not on the road (it resides on non-existing track)
    00378       // how should we handle that situation?
    00379       return false;
    00380     }
    00381     Yapf().SetOrigin(src_tile, TrackdirToTrackdirBits(src_td));
    00382     return true;
    00383   }
    00384
    00385   static Depot *stFindNearestDepot(const Vehicle *v, TileIndex tile, Trackdir td)
    00386   {
    00387     Tpf pf;
    00388     return pf.FindNearestDepot(v, tile, td);
    00389   }
    00390
    00391   FORCEINLINE Depot *FindNearestDepot(const Vehicle *v, TileIndex tile, Trackdir td)
    00392   {
    00393     // set origin and destination nodes
    00394     Yapf().SetOrigin(tile, TrackdirToTrackdirBits(td));
    00395
    00396     // find the best path
    00397     bool bFound = Yapf().FindPath(v);
    00398     if (!bFound) return false;
    00399
    00400     // some path found
    00401     // get found depot tile
    00402     Node *n = Yapf().GetBestNode();
    00403     TileIndex depot_tile = n->m_segment_last_tile;
    00404     assert(IsRoadDepotTile(depot_tile));
    00405     Depot *ret = GetDepotByTile(depot_tile);
    00406     return ret;
    00407   }
    00408 };
    00409
    00410 template <class Tpf_, class Tnode_list, template <class Types> class Tdestination>
    00411 struct CYapfRoad_TypesT
    00412 {
    00413   typedef CYapfRoad_TypesT<Tpf_, Tnode_list, Tdestination>  Types;
    00414
    00415   typedef Tpf_                              Tpf;
    00416   typedef CFollowTrackRoad                  TrackFollower;
    00417   typedef Tnode_list                        NodeList;
    00418   typedef CYapfBaseT<Types>                 PfBase;
    00419   typedef CYapfFollowRoadT<Types>           PfFollow;
    00420   typedef CYapfOriginTileT<Types>           PfOrigin;
    00421   typedef Tdestination<Types>               PfDestination;
    00422   typedef CYapfSegmentCostCacheNoneT<Types> PfCache;
    00423   typedef CYapfCostRoadT<Types>             PfCost;
    00424 };
    00425
    00426 struct CYapfRoad1         : CYapfT<CYapfRoad_TypesT<CYapfRoad1        , CRoadNodeListTrackDir, CYapfDestinationTileRoadT    > > {};
    00427 struct CYapfRoad2         : CYapfT<CYapfRoad_TypesT<CYapfRoad2        , CRoadNodeListExitDir , CYapfDestinationTileRoadT    > > {};
    00428
    00429 struct CYapfRoadAnyDepot1 : CYapfT<CYapfRoad_TypesT<CYapfRoadAnyDepot1, CRoadNodeListTrackDir, CYapfDestinationAnyDepotRoadT> > {};
    00430 struct CYapfRoadAnyDepot2 : CYapfT<CYapfRoad_TypesT<CYapfRoadAnyDepot2, CRoadNodeListExitDir , CYapfDestinationAnyDepotRoadT> > {};
    00431
    00432
    00433 Trackdir YapfChooseRoadTrack(const Vehicle *v, TileIndex tile, DiagDirection enterdir)
    00434 {
    00435   // default is YAPF type 2
    00436   typedef Trackdir (*PfnChooseRoadTrack)(const Vehicle*, TileIndex, DiagDirection);
    00437   PfnChooseRoadTrack pfnChooseRoadTrack = &CYapfRoad2::stChooseRoadTrack; // default: ExitDir, allow 90-deg
    00438
    00439   // check if non-default YAPF type should be used
    00440   if (_settings_game.pf.yapf.disable_node_optimization) {
    00441     pfnChooseRoadTrack = &CYapfRoad1::stChooseRoadTrack; // Trackdir, allow 90-deg
    00442   }
    00443
    00444   Trackdir td_ret = pfnChooseRoadTrack(v, tile, enterdir);
    00445   return td_ret;
    00446 }
    00447
    00448 uint YapfRoadVehDistanceToTile(const Vehicle *v, TileIndex tile)
    00449 {
    00450   // default is YAPF type 2
    00451   typedef uint (*PfnDistanceToTile)(const Vehicle*, TileIndex);
    00452   PfnDistanceToTile pfnDistanceToTile = &CYapfRoad2::stDistanceToTile; // default: ExitDir, allow 90-deg
    00453
    00454   // check if non-default YAPF type should be used
    00455   if (_settings_game.pf.yapf.disable_node_optimization) {
    00456     pfnDistanceToTile = &CYapfRoad1::stDistanceToTile; // Trackdir, allow 90-deg
    00457   }
    00458
    00459   // measure distance in YAPF units
    00460   uint dist = pfnDistanceToTile(v, tile);
    00461   // convert distance to tiles
    00462   if (dist != UINT_MAX) {
    00463     dist = (dist + YAPF_TILE_LENGTH - 1) / YAPF_TILE_LENGTH;
    00464   }
    00465
    00466   return dist;
    00467 }
    00468
    00469 Depot *YapfFindNearestRoadDepot(const Vehicle *v)
    00470 {
    00471   TileIndex tile = v->tile;
    00472   Trackdir trackdir = GetVehicleTrackdir(v);
    00473   if ((TrackStatusToTrackdirBits(GetTileTrackStatus(tile, TRANSPORT_ROAD, v->u.road.compatible_roadtypes)) & TrackdirToTrackdirBits(trackdir)) == 0) {
    00474     return NULL;
    00475   }
    00476
    00477   // handle the case when our vehicle is already in the depot tile
    00478   if (IsRoadDepotTile(tile)) {
    00479     // only what we need to return is the Depot*
    00480     return GetDepotByTile(tile);
    00481   }
    00482
    00483   // default is YAPF type 2
    00484   typedef Depot *(*PfnFindNearestDepot)(const Vehicle*, TileIndex, Trackdir);
    00485   PfnFindNearestDepot pfnFindNearestDepot = &CYapfRoadAnyDepot2::stFindNearestDepot;
    00486
    00487   // check if non-default YAPF type should be used
    00488   if (_settings_game.pf.yapf.disable_node_optimization) {
    00489     pfnFindNearestDepot = &CYapfRoadAnyDepot1::stFindNearestDepot; // Trackdir, allow 90-deg
    00490   }
    00491
    00492   Depot *ret = pfnFindNearestDepot(v, tile, trackdir);
    00493   return ret;
    00494 }