#include <msp/core/maputils.h>
+#include <msp/time/utils.h>
#include "catalogue.h"
#include "layout.h"
#include "route.h"
TrainRoutePlanner::TrainRoutePlanner(Layout &layout):
goal(0),
+ path_switch_bias(15*Time::sec),
+ timeout(10*Time::sec),
result(PENDING),
thread(0)
{
}
}
+void TrainRoutePlanner::set_timeout(const Time::TimeDelta &t)
+{
+ timeout = t;
+}
+
TrainRoutePlanner::Result TrainRoutePlanner::plan()
{
prepare_plan();
void TrainRoutePlanner::create_plan()
{
+ Time::TimeStamp timeout_stamp = Time::now()+timeout;
+ unsigned count = 0;
while(!queue.empty())
{
const RoutingStep &step = get_step();
}
add_steps(step);
+
+ if(++count>=1000)
+ {
+ if(Time::now()>timeout_stamp)
+ break;
+ count = 0;
+ }
}
result = FAILED;
{
list<RoutingStep> new_steps;
step.create_successors(new_steps);
+ if(new_steps.empty())
+ return;
+
new_steps.sort();
+ if(!queue.empty() && new_steps.front().cost_estimate<queue.front().cost_estimate+path_switch_bias)
+ new_steps.front().preferred = true;
queue.merge(new_steps);
}
for(vector<TrainRoutingState>::const_iterator j=i->trains.begin(); j!=i->trains.end(); ++j)
{
Track **history = j->info->track_history;
+ // Don't process the same track again.
if(j->track.track()==history[0])
continue;
bool start_new_route = true;
if(!j->info->routes.empty())
{
+ /* If we already have a route and this track or any linked track is
+ in it, start a new one to avoid loops. */
route = j->info->routes.front();
start_new_route = route->has_track(*j->track);
if(!start_new_route)
route = new Route(j->info->train->get_layout());
route->set_name("Router");
route->set_temporary(true);
+ /* Have the routes overlap by two tracks to ensure that turnout
+ paths can be deduced. */
for(unsigned k=0; (k<2 && history[k]); ++k)
route->add_track(*history[k]);
j->info->routes.push_front(route);
map<Track *, TrainRouter::SequencePoint *>::iterator k = sequenced_tracks.find(j->track.track());
if(k!=sequenced_tracks.end())
{
+ // Add a sequence point if another train uses this track afterwards.
if(!k->second->preceding_train)
{
k->second->preceding_train = j->info->train;
}
else if(waitable)
{
+ /* Create a sequence point if it's possible to wait and let another
+ train past. */
j->info->sequence.push_front(TrainRouter::SequencePoint(j->track->get_block(), sequence));
sequenced_tracks[j->track.track()] = &j->info->sequence.front();
--sequence;
TrainRoutePlanner::TrainRoutingInfo::TrainRoutingInfo(Train &t):
train(&t),
+ length(0),
speed(train->get_maximum_speed()),
- first_noncritical(train->get_first_noncritical_block().block()),
+ first_noncritical(train->get_last_critical_block().next().block()),
router(train->get_ai_of_type<TrainRouter>()),
- waypoints(router ? router->get_n_waypoints() : 0),
has_duration(false)
{
- if(!waypoints.empty())
+ if(unsigned n_wps = router->get_n_waypoints())
{
- metrics.resize(waypoints.size());
- for(unsigned i=0; i<waypoints.size(); ++i)
+ waypoints.reserve(n_wps),
+ metrics.reserve(n_wps);
+ for(unsigned i=0; i<n_wps; ++i)
{
- waypoints[i] = &router->get_waypoint(i);
- metrics[i] = &router->get_metric(i);
+ waypoints.push_back(router->get_waypoint(i));
+ metrics.push_back(&router->get_metric(i));
}
has_duration = router->get_trip_duration();
}
+ unsigned n_vehs = train->get_n_vehicles();
+ for(unsigned i=0; i<n_vehs; ++i)
+ length += train->get_vehicle(i).get_type().get_length();
+
// If no maximum speed is specified, use a sensible default
if(!speed)
speed = 20*train->get_layout().get_catalogue().get_scale();
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;
Time::TimeDelta TrainRoutePlanner::TrainRoutingState::get_time_to_next_track() const
{
- return ((track->get_type().get_path_length(path)-offset)/info->speed)*Time::sec+delay;
+ return ((occupied_tracks->path_length-offset)/info->speed)*Time::sec+delay;
+}
+
+Time::TimeDelta TrainRoutePlanner::TrainRoutingState::get_time_to_pass(Track &trk) const
+{
+ if(is_occupying(trk))
+ {
+ float passed_length = 0;
+ for(const OccupiedTrack *occ=occupied_tracks; (occ && occ->track!=&trk); occ=occ->next)
+ passed_length += occ->path_length;
+ return (max(info->length-passed_length, 0.0f)/info->speed)*Time::sec+delay;
+ }
+
+ 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->length)/info->speed)*Time::sec+delay;
+ }
+
+ return Time::day;
}
bool TrainRoutePlanner::TrainRoutingState::is_occupying(Track &trk) const
{
TrackIter next_track = track.next(path);
- const TrackChain *wp_chain = info->waypoints[waypoint];
- if(wp_chain->has_track(*track) && !wp_chain->has_track(*next_track))
- {
- if(waypoint+1<info->waypoints.size())
- ++waypoint;
- else
+ // Check if we're about the exit the current waypoint's tracks.
+ const TrainRouter::Waypoint &wp = info->waypoints[waypoint];
+ if(wp.chain->has_track(*track) && !wp.chain->has_track(*next_track))
+ if(wp.direction==TrackChain::UNSPECIFIED || track==wp.chain->iter_for(*track, wp.direction))
{
- state = ARRIVED;
- return true;
+ if(waypoint+1<info->waypoints.size())
+ ++waypoint;
+ else
+ {
+ state = ARRIVED;
+ return true;
+ }
}
- }
- if(info->first_noncritical->has_track(*track))
+ // If we're entering the first non-critical block, clear the critical flag.
+ if(info->first_noncritical->has_track(*next_track))
critical = false;
return false;
offset += distance;
back_offset += distance;
+ // See if the tail end of the train has passed any sensors.
unsigned count_to_free = 0;
unsigned last_sensor_addr = 0;
float distance_after_sensor = 0;
occ = occ->next;
}
+ // Free the last passed sensor and any tracks behind it.
if(count_to_free && back_offset>distance_after_sensor)
{
back_offset -= distance_after_sensor;
}
float secs = dt/Time::sec;
+ // There may be negative delay remaining after previous step.
if(delay)
{
secs -= delay/Time::sec;
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)
offset = 0;
}
+void TrainRoutePlanner::TrainRoutingState::set_path(unsigned p)
+{
+ path = p;
+ OccupiedTrack *next_occ = occupied_tracks->next;
+ if(!--occupied_tracks->refcount)
+ delete occupied_tracks;
+ occupied_tracks = new OccupiedTrack(*track, path, next_occ);
+ update_estimate();
+}
+
void TrainRoutePlanner::TrainRoutingState::update_estimate()
{
TrackIter iter = track.reverse(path);
- float distance = info->metrics[waypoint]->get_distance_from(*iter.track(), iter.entry());
- distance += track->get_type().get_path_length(path)-offset;
- remaining_estimate = distance;
+ remaining_estimate = info->metrics[waypoint]->get_distance_from(*iter.track(), iter.entry());
+ if(remaining_estimate>=0)
+ remaining_estimate += occupied_tracks->path_length-offset;
}
bool TrainRoutePlanner::TrainRoutingState::is_viable() const
TrainRoutePlanner::RoutingStep::RoutingStep():
+ preferred(false),
prev(0)
{ }
TrainRoutePlanner::RoutingStep::RoutingStep(const RoutingStep *p):
time(p->time),
- penalty(p->penalty),
cost_estimate(p->cost_estimate),
+ preferred(false),
trains(p->trains),
prev(p)
{ }
void TrainRoutePlanner::RoutingStep::create_successors(list<RoutingStep> &new_steps) const
{
RoutingStep next(this);
- if(next.update_states())
- {
- if(next.check_deadlocks())
- return;
-
- new_steps.push_back(next);
+ if(next.update_states() && next.check_deadlocks())
return;
- }
int train_index = find_next_train();
if(train_index<0)
Time::TimeDelta dt = train.get_time_to_next_track();
next.advance(dt);
+ /* Check arrival after the train has advanced to the end of its current track
+ so travel time and occupied tracks will be correct. */
if(train.check_arrival())
{
new_steps.push_back(next);
const TrackType::Endpoint &entry_ep = train.track.endpoint();
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);
+ /* Only create a successor step matching the currently set path for a
+ critical track. */
+ unsigned critical_path = train.track->get_type().coerce_path(train.track.entry(), train.track->get_active_path());
+ create_successor(next, train_index, critical_path, new_steps);
}
else
{
+ // Create successor steps for all possible paths through the new track.
for(unsigned i=0; entry_ep.paths>>i; ++i)
if(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();
+ create_successor(next, train_index, i, new_steps);
}
if(entry_ep.paths!=train.track->get_type().get_paths() && !train.critical)
{
+ /* Create a waiting state before the track if there's at least one path
+ that doesn't pass through the entry endpoint. */
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);
}
}
+void TrainRoutePlanner::RoutingStep::create_successor(RoutingStep &next, unsigned train_index, unsigned path, list<RoutingStep> &new_steps)
+{
+ TrainRoutingState &train = next.trains[train_index];
+
+ train.set_path(path);
+ next.update_estimate();
+ if(next.is_viable())
+ new_steps.push_back(next);
+}
+
bool TrainRoutePlanner::RoutingStep::update_states()
{
bool changes = false;
i->blocked_by = get_occupant(*next_track);
if(i->blocked_by>=0)
{
+ /* If the train is still traversing its last critical track, the
+ flag needs to be cleared here to pass viability test. */
if(i->info->first_noncritical->has_track(*next_track))
i->critical = false;
+
+ if(i->state!=BLOCKED)
+ i->estimated_wait = trains[i->blocked_by].get_time_to_pass(*next_track);
+
+ /* Trains in the WAITING state will also transition to BLOCKED and
+ then to MOVING when the other train has passed. */
i->state = BLOCKED;
}
else if(i->state==BLOCKED)
+ {
+ i->estimated_wait = Time::zero;
i->state = MOVING;
+ }
}
else
i->state = BLOCKED;
if(i->state!=BLOCKED)
continue;
+ // A train blocked by end of track is always considered a deadlock.
if(i->blocked_by<0)
return true;
+ /* Use the tortoise and hare algorithm to check if trains are blocked
+ cyclically (A blocks B, which blocks ..., which blocks A). */
int slow = i->blocked_by;
int fast = trains[slow].blocked_by;
while(fast>=0 && trains[fast].blocked_by>=0)
void TrainRoutePlanner::RoutingStep::update_estimate()
{
- cost_estimate = penalty;
+ cost_estimate = Time::zero;
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
bool TrainRoutePlanner::RoutingStep::operator<(const RoutingStep &other) const
{
+ if(preferred!=other.preferred)
+ return preferred>other.preferred;
return cost_estimate<other.cost_estimate;
}
projects / r2c2.git / blobdiff