Add an actual heuristic for wait time

[r2c2.git] / source / libr2c2 / trainrouteplanner.cpp

diff --git a/source/libr2c2/trainrouteplanner.cpp b/source/libr2c2/trainrouteplanner.cpp
index 7d1b0c230292d66b48129ce0a60fb3f892d884fb..e083087ba1b64354bef11f662dd4ea2bc36cf011 100644 (file)
--- a/source/libr2c2/trainrouteplanner.cpp
+++ b/source/libr2c2/trainrouteplanner.cpp
@@ -338,6 +338,7 @@ TrainRoutePlanner::TrainRoutingState::TrainRoutingState(const TrainRoutingState
        distance_traveled(other.distance_traveled),
        remaining_estimate(other.remaining_estimate),
        wait_time(other.wait_time),
+       estimated_wait(other.estimated_wait),
        blocked_by(other.blocked_by)
 {
        ++occupied_tracks->refcount;
@@ -354,6 +355,21 @@ Time::TimeDelta TrainRoutePlanner::TrainRoutingState::get_time_to_next_track() c
        return ((track->get_type().get_path_length(path)-offset)/info->speed)*Time::sec+delay;
 }
 
+Time::TimeDelta TrainRoutePlanner::TrainRoutingState::get_time_to_pass(Track &trk) const
+{
+       if(is_occupying(trk))
+               return Time::zero;
+
+       for(unsigned wp=waypoint; wp<info->waypoints.size(); ++wp)
+       {
+               float distance = info->metrics[wp]->get_distance_from(trk);
+               if(distance>=0 && distance<remaining_estimate)
+                       return ((remaining_estimate-distance)/info->speed)*Time::sec+delay;
+       }
+
+       return Time::day;
+}
+
 bool TrainRoutePlanner::TrainRoutingState::is_occupying(Track &trk) const
 {
        if(state==ARRIVED && !duration && info->has_duration)
@@ -454,6 +470,9 @@ void TrainRoutePlanner::TrainRoutingState::advance(const Time::TimeDelta &dt)
        if(duration)
                duration = max(duration-secs*Time::sec, Time::zero);
 
+       if(estimated_wait)
+               estimated_wait = max(estimated_wait-secs*Time::sec, Time::zero);
+
        if(state==MOVING)
                advance(info->speed*secs);
        else if(state!=ARRIVED)
@@ -555,7 +574,16 @@ void TrainRoutePlanner::RoutingStep::create_successors(list<RoutingStep> &new_st
                RoutingStep wait(this);
                wait.advance(dt);
                wait.trains[train_index].state = WAITING;
-               wait.penalty += 15*Time::sec;
+
+               Time::TimeDelta estimated_wait = Time::day;
+               for(unsigned i=0; i<wait.trains.size(); ++i)
+                       if(i!=static_cast<unsigned>(train_index) && wait.trains[i].state!=ARRIVED)
+                       {
+                               Time::TimeDelta ttp = wait.trains[i].get_time_to_pass(*train.track);
+                               estimated_wait = min(estimated_wait, ttp);
+                       }
+               wait.trains[train_index].estimated_wait = estimated_wait;
+
                wait.update_estimate();
                if(wait.is_viable())
                        new_steps.push_back(wait);
@@ -678,7 +706,7 @@ void TrainRoutePlanner::RoutingStep::update_estimate()
        cost_estimate = penalty;
        for(vector<TrainRoutingState>::const_iterator i=trains.begin(); i!=trains.end(); ++i)
                if(i->remaining_estimate>=0)
-                       cost_estimate += i->wait_time+((i->distance_traveled+i->remaining_estimate)/i->info->speed)*Time::sec;
+                       cost_estimate += i->wait_time+i->estimated_wait+((i->distance_traveled+i->remaining_estimate)/i->info->speed)*Time::sec;
 }
 
 bool TrainRoutePlanner::RoutingStep::is_viable() const