X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flibmarklin%2Ftrain.cpp;h=acf8aa51486de42adb8901b98f9c78f2d0b79ac0;hb=97443d96ff3ce51388d2edd1e0dca8f2cd231346;hp=d87d3768a7e0a482db808d934cd5f5211f389197;hpb=182ee232f01d5bb25ddc4cf6e00249698871412f;p=r2c2.git diff --git a/source/libmarklin/train.cpp b/source/libmarklin/train.cpp index d87d376..acf8aa5 100644 --- a/source/libmarklin/train.cpp +++ b/source/libmarklin/train.cpp @@ -24,6 +24,19 @@ Distributed under the GPL using namespace std; using namespace Msp; +namespace { + +struct SetFlag +{ + bool &flag; + + SetFlag(bool &f): flag(f) { flag = true; } + ~SetFlag() { flag = false; } +}; + +} + + namespace Marklin { Train::Train(Layout &l, const VehicleType &t, unsigned a): @@ -31,7 +44,10 @@ 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), controller(new AIControl(*this, new SimpleController)), timetable(0), active(false), @@ -90,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); @@ -200,7 +221,7 @@ void Train::set_route(const Route *r) next_route = 0; end_of_route = false; - if(route) + if(route && !cur_blocks.empty()) { BlockRef &last = (rsv_blocks.empty() ? cur_blocks.back() : rsv_blocks.back()); BlockRef next = last.next(); @@ -272,12 +293,16 @@ void Train::place(Block &block, unsigned entry) else { const Block::Endpoint &bep = block.get_endpoints()[entry]; - vehicles.front()->place(bep.track, bep.track_ep, 0, Vehicle::BACK_BUFFER); + vehicles.back()->place(bep.track, bep.track_ep, 0, Vehicle::BACK_BUFFER); } } 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) { @@ -308,49 +333,7 @@ int Train::get_entry_to_block(Block &block) const float Train::get_reserved_distance() const { - 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) @@ -407,7 +390,10 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt) float d = get_real_speed(current_speed)*(dt/Time::sec); if(ok) + { + SetFlag setf(advancing); vehicle.advance(reverse ? -d : d); + } else if(accurate_position) { overshoot_dist += d; @@ -426,44 +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()); + float dist = get_reserved_distance_until(cur_blocks.front().block, true); - 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()-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; - - 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()); @@ -589,7 +540,7 @@ void Train::sensor_event(unsigned addr, bool state) j->block->traverse(j->entry, &block_len); travel_dist += block_len; - if(j->block->get_sensor_id()==addr) + if(j->block->get_sensor_id()==addr && !advancing) { const Block::Endpoint &bep = j->block->get_endpoints()[j->entry]; if(reverse) @@ -648,7 +599,7 @@ void Train::sensor_event(unsigned addr, bool state) } } - if(end!=cur_blocks.begin()) + if(end!=cur_blocks.begin() && end!=cur_blocks.end()) // Free blocks up to the last inactive sensor release_blocks(cur_blocks, cur_blocks.begin(), end); } @@ -661,7 +612,12 @@ void Train::turnout_event(unsigned addr, bool) unsigned pending_addr = pending_block->get_turnout_id(); bool double_addr = (*pending_block->get_tracks().begin())->get_type().is_double_address(); if(addr==pending_addr || (double_addr && addr==pending_addr+1)) - reserve_more(); + { + if(reserving) + pending_block = 0; + else + reserve_more(); + } } } @@ -682,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; @@ -704,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)) @@ -714,13 +679,23 @@ unsigned Train::reserve_more() } } - bool got_more = false; - BlockRef *good = last; + float approach_margin = 50*layout.get_catalogue().get_scale(); + float min_dist = controller->get_braking_distance()*1.3+approach_margin*2; + + 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; @@ -738,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; } } @@ -749,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; } @@ -785,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; @@ -802,31 +837,38 @@ unsigned Train::reserve_more() if(path!=static_cast(entry_ep.track->get_active_path())) { // The turnout is set to wrong path - switch and wait for it - link->reserve(0); pending_block = link; entry_ep.track->set_active_path(path); - break; + if(pending_block) + { + link->reserve(0); + break; + } } } + 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; for(i=rsv_blocks.begin(); (i!=rsv_blocks.end() && !i->block->get_sensor_id()); ++i) ; @@ -836,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)