Don't diverge from critical blocks

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

diff --git a/source/libr2c2/trainrouteplanner.cpp b/source/libr2c2/trainrouteplanner.cpp
index 163bb281eacd019997e8975802f670a0d904ce6f..a05e97ea7fb2fed688651a3b6b81c104d3d5d19c 100644 (file)
--- a/source/libr2c2/trainrouteplanner.cpp
+++ b/source/libr2c2/trainrouteplanner.cpp
@@ -29,7 +29,11 @@ TrainRoutePlanner::TrainRoutePlanner(Layout &layout):
 
 TrainRoutePlanner::~TrainRoutePlanner()
 {
-       delete thread;
+       if(thread)
+       {
+               thread->join();
+               delete thread;
+       }
 }
 
 TrainRoutePlanner::Result TrainRoutePlanner::plan()
@@ -55,9 +59,13 @@ TrainRoutePlanner::Result TrainRoutePlanner::check()
 {
        if(result==PENDING && goal)
        {
+               if(thread)
+               {
+                       thread->join();
+                       delete thread;
+                       thread = 0;
+               }
                finalize_plan();
-               delete thread;
-               thread = 0;
        }
 
        return result;
@@ -206,6 +214,7 @@ void TrainRoutePlanner::finalize_plan()
 TrainRoutePlanner::TrainRoutingInfo::TrainRoutingInfo(Train &t):
        train(&t),
        speed(train->get_maximum_speed()),
+       first_noncritical(train->get_first_noncritical_block().block()),
        router(train->get_ai_of_type<TrainRouter>()),
        destination(0),
        has_duration(false)
@@ -213,15 +222,18 @@ TrainRoutePlanner::TrainRoutingInfo::TrainRoutingInfo(Train &t):
        if(router)
        {
                destination = router->get_destination();
-               waypoints.resize(router->get_n_waypoints());
-               metrics.resize(waypoints.size()+1);
-               metrics[0] = &router->get_metric(-1);
-               for(unsigned i=0; i<waypoints.size(); ++i)
+               if(destination)
                {
-                       waypoints[i] = &router->get_waypoint(i);
-                       metrics[i+1] = &router->get_metric(i);
+                       waypoints.resize(router->get_n_waypoints());
+                       metrics.resize(waypoints.size()+1);
+                       metrics[0] = &router->get_metric(-1);
+                       for(unsigned i=0; i<waypoints.size(); ++i)
+                       {
+                               waypoints[i] = &router->get_waypoint(i);
+                               metrics[i+1] = &router->get_metric(i);
+                       }
+                       has_duration = router->get_trip_duration();
                }
-               has_duration = router->get_trip_duration();
        }
 
        // If no maximum speed is specified, use a sensible default
@@ -261,6 +273,7 @@ TrainRoutePlanner::OccupiedTrack::~OccupiedTrack()
 
 TrainRoutePlanner::TrainRoutingState::TrainRoutingState(TrainRoutingInfo &inf):
        info(&inf),
+       critical(true),
        occupied_tracks(0),
        state(MOVING),
        delay(info->router->get_departure_delay()),
@@ -296,6 +309,7 @@ TrainRoutePlanner::TrainRoutingState::TrainRoutingState(const TrainRoutingState
        info(other.info),
        track(other.track),
        path(other.path),
+       critical(other.critical),
        occupied_tracks(other.occupied_tracks),
        offset(other.offset),
        back_offset(other.back_offset),
@@ -350,6 +364,9 @@ bool TrainRoutePlanner::TrainRoutingState::check_arrival()
                        waypoint = -1;
        }
 
+       if(info->first_noncritical->has_track(*track))
+               critical = false;
+
        return false;
 }
 
@@ -427,6 +444,7 @@ TrainRoutePlanner::RoutingStep::RoutingStep():
 
 TrainRoutePlanner::RoutingStep::RoutingStep(const RoutingStep *p):
        time(p->time),
+       penalty(p->penalty),
        cost_estimate(p->cost_estimate),
        trains(p->trains),
        prev(p)
@@ -463,21 +481,41 @@ void TrainRoutePlanner::RoutingStep::create_successors(list<RoutingStep> &new_st
        train.advance_track(0);
 
        const TrackType::Endpoint &next_entry_ep = next_track.endpoint();
-       for(unsigned i=0; next_entry_ep.paths>>i; ++i)
-               if(next_entry_ep.has_path(i))
-               {
-                       train.path = i;
-                       train.update_estimate();
-                       next.update_estimate();
-                       if(next.is_viable())
-                               new_steps.push_back(next);
-               }
+       if(train.critical)
+       {
+               train.path = train.track->get_type().coerce_path(train.track.entry(), train.track->get_active_path());
+               train.update_estimate();
+               next.update_estimate();
+               if(next.is_viable())
+                       new_steps.push_back(next);
+       }
+       else
+       {
+               for(unsigned i=0; next_entry_ep.paths>>i; ++i)
+                       if(next_entry_ep.has_path(i))
+                       {
+                               train.path = i;
+                               train.update_estimate();
+                               next.update_estimate();
+                               if(next.is_viable())
+                                       new_steps.push_back(next);
+                       }
+       }
+
+       new_steps.sort();
+       for(list<RoutingStep>::iterator i=new_steps.begin(); ++i!=new_steps.end(); )
+       {
+               i->penalty += 5*Time::sec;
+               i->update_estimate();
+       }
 
-       if(next_entry_ep.paths!=next_track->get_type().get_paths())
+       if(next_entry_ep.paths!=next_track->get_type().get_paths() && !train.critical)
        {
                RoutingStep wait(this);
                wait.advance(dt);
                wait.trains[train_index].state = WAITING;
+               wait.penalty += 15*Time::sec;
+               wait.update_estimate();
                if(wait.is_viable())
                        new_steps.push_back(wait);
        }
@@ -573,7 +611,7 @@ void TrainRoutePlanner::RoutingStep::advance(const Time::TimeDelta &dt)
 
 void TrainRoutePlanner::RoutingStep::update_estimate()
 {
-       cost_estimate = Time::zero;
+       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;