X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flibmarklin%2Ftrain.cpp;h=acf8aa51486de42adb8901b98f9c78f2d0b79ac0;hb=97443d96ff3ce51388d2edd1e0dca8f2cd231346;hp=8166c09a86944d1bb4251dec61a38819d08b8920;hpb=3d38bcbe8add00b558012fa0cd4e0be26464dbeb;p=r2c2.git diff --git a/source/libmarklin/train.cpp b/source/libmarklin/train.cpp index 8166c09..acf8aa5 100644 --- a/source/libmarklin/train.cpp +++ b/source/libmarklin/train.cpp @@ -44,6 +44,7 @@ Train::Train(Layout &l, const VehicleType &t, unsigned a): loco_type(t), address(a), priority(0), + yielding_to(0), pending_block(0), reserving(false), advancing(false), @@ -105,6 +106,11 @@ void Train::set_priority(int p) priority = p; } +void Train::yield_to(const Train &t) +{ + yielding_to = &t; +} + void Train::add_vehicle(const VehicleType &vt) { Vehicle *veh = new Vehicle(layout, vt); @@ -293,6 +299,10 @@ void Train::place(Block &block, unsigned entry) bool Train::free_block(Block &block) { + float margin = 10*layout.get_catalogue().get_scale(); + if(get_reserved_distance_until(&block, false)get_braking_distance()*1.3+margin) + return false; + unsigned nsens = 0; for(list::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i) { @@ -323,52 +333,7 @@ int Train::get_entry_to_block(Block &block) const float Train::get_reserved_distance() const { - if(cur_blocks.empty()) - return 0; - - Vehicle &veh = *(reverse ? vehicles.back() : vehicles.front()); - const VehicleType &vtype = veh.get_type(); - - Track *track = veh.get_track(); - if(!track) - return 0; - unsigned entry = veh.get_entry(); - - float result = -vtype.get_length()/2; - if(reverse) - { - entry = track->traverse(entry); - result += veh.get_offset(); - } - else - result -= veh.get_offset(); - - bool first = true; - list::const_iterator block = cur_blocks.begin(); - while(1) - { - if(!first || !reverse) - result += track->get_type().get_path_length(track->get_active_path()); - first = false; - - if(track->get_type().get_endpoints().size()<2) - return result; - - unsigned exit = track->traverse(entry); - Track *next = track->get_link(exit); - - while(!block->block->get_tracks().count(next)) - { - ++block; - if(block==cur_blocks.end()) - block = rsv_blocks.begin(); - if(block==rsv_blocks.end()) - return result; - } - - entry = next->get_endpoint_by_link(*track); - track = next; - } + return get_reserved_distance_until(0, false); } void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt) @@ -447,47 +412,9 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt) if(!cur_blocks.empty() && !cur_blocks.front().block->get_sensor_id()) { - Vehicle &veh = *(reverse ? vehicles.front() : vehicles.back()); - - list::iterator i = cur_blocks.begin(); - const Block::Endpoint &bep = i->block->get_endpoints()[i->entry]; - - Track *track = bep.track; - unsigned entry = bep.track_ep; - - bool found = false; - float dist = veh.get_offset(); - if(reverse) - dist = veh.get_track()->get_type().get_path_length(veh.get_track()->get_active_path())-dist; - dist -= veh.get_type().get_length()/2; - while(1) - { - if(track==veh.get_track()) - { - found = true; - break; - } - - if(i!=cur_blocks.begin()) - { - float path_len = track->get_type().get_path_length(track->get_active_path()); - dist += path_len; - } - - unsigned exit = track->traverse(entry); - Track *next = track->get_link(exit); - entry = next->get_endpoint_by_link(*track); - track = next; + float dist = get_reserved_distance_until(cur_blocks.front().block, true); - if(!i->block->get_tracks().count(track)) - { - ++i; - if(i==cur_blocks.end()) - break; - } - } - - if(found && i!=cur_blocks.begin() && dist>10*layout.get_catalogue().get_scale()) + if(dist>10*layout.get_catalogue().get_scale()) { cur_blocks.front().block->reserve(0); cur_blocks.erase(cur_blocks.begin()); @@ -711,21 +638,30 @@ unsigned Train::reserve_more() if(!active) return 0; - BlockRef *last = 0; + BlockRef *start = 0; if(!rsv_blocks.empty()) - last = &rsv_blocks.back(); + start = &rsv_blocks.back(); else if(!cur_blocks.empty()) - last = &cur_blocks.back(); - if(!last) + start = &cur_blocks.back(); + if(!start) return 0; pending_block = 0; - // See how many sensor blocks we already have + // See how many sensor blocks and how much track we already have unsigned nsens = 0; + float dist = 0; for(list::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i) + { if(i->block->get_sensor_id()) ++nsens; + if(nsens>0) + { + float length = 0; + i->block->traverse(i->entry, &length); + dist += length; + } + } if(end_of_route) return nsens; @@ -733,7 +669,7 @@ unsigned Train::reserve_more() const Route *cur_route = 0; if(route) { - const set &tracks = last->block->get_tracks(); + const set &tracks = start->block->get_tracks(); for(set::const_iterator i=tracks.begin(); (cur_route!=route && i!=tracks.end()); ++i) { if(route->get_tracks().count(*i)) @@ -743,15 +679,23 @@ unsigned Train::reserve_more() } } - SetFlag setf(reserving); + float approach_margin = 50*layout.get_catalogue().get_scale(); + float min_dist = controller->get_braking_distance()*1.3+approach_margin*2; - bool got_more = false; - BlockRef *good = last; + BlockRef *last = start; + BlockRef *good = start; unsigned good_sens = nsens; - while(good_sens<3) + float good_dist = dist; + Train *blocking_train = 0; + std::list contested_blocks; + + SetFlag setf(reserving); + + while(good_sens<3 || good_distblock->traverse(last->entry); + float length = 0; + unsigned exit = last->block->traverse(last->entry, cur_route, &length); Block *link = last->block->get_link(exit); if(!link) break; @@ -769,9 +713,13 @@ unsigned Train::reserve_more() else if(!cur_route->get_tracks().count(entry_ep.track)) { // Keep the blocks if we arrived at the end of the route - good = last; - good_sens = nsens; - end_of_route = true; + if(!blocking_train) + { + good = last; + good_sens = nsens; + good_dist = dist; + end_of_route = true; + } break; } } @@ -780,30 +728,85 @@ unsigned Train::reserve_more() if(link->get_endpoints().size()<2) { - good = last; - good_sens = nsens; + if(!blocking_train) + { + good = last; + good_sens = nsens; + good_dist = dist; + } break; } + if(blocking_train) + { + if(link->get_train()!=blocking_train) + { + // XXX is it possible that this won't free all the blocks we want? + if(blocking_train->free_block(*contested_blocks.back().block)) + { + // Roll back and start actually reserving the blocks + last = &rsv_blocks.back(); + if(blocking_train->get_priority()==priority) + blocking_train->yield_to(*this); + blocking_train = 0; + continue; + } + else + { + pending_block = contested_blocks.front().block; + break; + } + } + else + { + contested_blocks.push_back(BlockRef(link, entry)); + last = &contested_blocks.back(); + continue; + } + } + bool reserved = link->reserve(this); if(!reserved) { - // Ask a lesser priority train to free the block for us - if(link->get_train()->get_priority()get_train()->free_block(*link)) - reserved = link->reserve(this); + /* We've found another train. If it wants to exit the block from the + same endpoint we're trying to enter from or the other way around, + treat it as coming towards us. Otherwise treat it as going in the + same direction. */ + Train *other_train = link->get_train(); + int other_entry = other_train->get_entry_to_block(*link); + if(other_entry<0) + throw LogicError("Block reservation inconsistency"); + + int other_prio = other_train->get_priority(); + + bool entry_conflict = (static_cast(entry)==link->traverse(other_entry)); + bool exit_conflict = (link->traverse(entry)==static_cast(other_entry)); + if(!entry_conflict && !exit_conflict) + { + /* Same direction, keep the blocks we got so far and wait for the + other train to pass */ + good = last; + good_sens = nsens; + good_dist = dist; + + // Ask a lesser priority train to free the block for us + if(other_train->get_priority()free_block(*link)) + reserved = link->reserve(this); + } + else if(other_prioget_train()->get_entry_to_block(*link); - if(other_entry<0) - throw LogicError("Block reservation inconsistency"); - if(static_cast(entry)!=link->traverse(other_entry)) - { - good = last; - good_sens = nsens; - } pending_block = link; break; } @@ -816,6 +819,7 @@ unsigned Train::reserve_more() // Keep the blocks reserved so far, as either us or the other train can diverge good = last; good_sens = nsens; + good_dist = dist; // Figure out what path we'd like to take on the turnout int path = -1; @@ -843,24 +847,27 @@ unsigned Train::reserve_more() } } + if(!contested_blocks.empty() && contested_blocks.front().block==link) + contested_blocks.pop_front(); + rsv_blocks.push_back(BlockRef(link, entry)); last = &rsv_blocks.back(); if(last->block->get_sensor_id()) - { ++nsens; - got_more = true; - } + if(nsens>0) + dist += length; } // Unreserve blocks that were not good - while(!rsv_blocks.empty() && last!=good) + while(!rsv_blocks.empty() && &rsv_blocks.back()!=good) { - last->block->reserve(0); + rsv_blocks.back().block->reserve(0); rsv_blocks.erase(--rsv_blocks.end()); - if(!rsv_blocks.empty()) - last = &rsv_blocks.back(); } + if(!rsv_blocks.empty() && &rsv_blocks.back()!=start) + // We got some new blocks, so no longer need to yield + yielding_to = 0; // Make any sensorless blocks at the beginning immediately current list::iterator i; @@ -871,6 +878,70 @@ unsigned Train::reserve_more() return good_sens; } +float Train::get_reserved_distance_until(const Block *until_block, bool back) const +{ + if(cur_blocks.empty()) + return 0; + + Vehicle &veh = *(reverse!=back ? vehicles.back() : vehicles.front()); + const VehicleType &vtype = veh.get_type(); + + Track *track = veh.get_track(); + if(!track) + return 0; + + list::const_iterator block = cur_blocks.begin(); + while(block!=cur_blocks.end() && !block->block->get_tracks().count(track)) + ++block; + if(block==cur_blocks.end() || block->block==until_block) + return 0; + + unsigned entry = veh.get_entry(); + + float result = veh.get_offset(); + if(reverse!=back) + entry = track->traverse(entry); + else + result = track->get_type().get_path_length(track->get_active_path())-result; + result -= vtype.get_length()/2; + + while(1) + { + if(track->get_type().get_endpoints().size()<2) + break; + + Track *next = track->get_link(track->traverse(entry)); + + if(!block->block->get_tracks().count(next)) + { + if(back) + { + if(block==cur_blocks.begin()) + break; + --block; + } + else + { + ++block; + if(block==cur_blocks.end()) + block = rsv_blocks.begin(); + if(block==rsv_blocks.end()) + break; + } + + if(block->block==until_block) + break; + } + + entry = next->get_endpoint_by_link(*track); + track = next; + + result += track->get_type().get_path_length(track->get_active_path()); + } + + return result; +} + float Train::get_real_speed(unsigned i) const { if(real_speed[i].weight)