X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flibmarklin%2Fvehicle.cpp;h=b54b531e0de2964e9fe1fb87590d4d3835ed4da7;hb=9b05c573a38639827697fe393d55b7c76f5bde45;hp=6011537d02b3205a4996a89a80edad272cfe2a2e;hpb=e94014530a6e28a42bc5678e579ee07e0ee5329b;p=r2c2.git diff --git a/source/libmarklin/vehicle.cpp b/source/libmarklin/vehicle.cpp index 6011537..b54b531 100644 --- a/source/libmarklin/vehicle.cpp +++ b/source/libmarklin/vehicle.cpp @@ -7,13 +7,16 @@ Distributed under the GPL #include #include "catalogue.h" +#include "driver.h" #include "layout.h" #include "track.h" +#include "trackiter.h" #include "tracktype.h" #include "vehicle.h" #include "vehicletype.h" using namespace std; +using namespace Msp; namespace Marklin { @@ -22,56 +25,107 @@ Vehicle::Vehicle(Layout &l, const VehicleType &t): type(t), next(0), prev(0), - direction(0) + direction(0), + bogie_dirs(type.get_bogies().size()), + front_sensor(0), + back_sensor(0) { layout.add_vehicle(*this); } Vehicle::~Vehicle() { + if(next) + detach_back(); + if(prev) + detach_front(); layout.remove_vehicle(*this); } -void Vehicle::place(Track *t, unsigned e, float o, PlaceMode m) +void Vehicle::attach_back(Vehicle &veh) { - track_pos = TrackPosition(t, e, o); + if(next || veh.prev) + throw InvalidState("Already attached"); + + next = &veh; + veh.prev = this; + + if(track_pos.track) + propagate_backward(); +} + +void Vehicle::attach_front(Vehicle &veh) +{ + if(prev || veh.next) + throw InvalidState("Already attached"); + + prev = &veh; + veh.next = this; + + if(prev->get_track()) + prev->propagate_backward(); +} + +void Vehicle::detach_back() +{ + if(!next) + throw InvalidState("Not attached"); + + next->prev = 0; + next = 0; +} + +void Vehicle::detach_front() +{ + if(!prev) + throw InvalidState("Not attached"); + + prev->next = 0; + prev = 0; +} + +void Vehicle::place(Track &t, unsigned e, float o, PlaceMode m) +{ + track_pos = TrackPosition(&t, e, o); if(m==FRONT_AXLE) - { - float front = type.get_length()/2; - if(!type.get_axles().empty()) - front = type.get_axles().front().position; - if(!type.get_bogies().empty()) - { - const VehicleType::Bogie &bogie = type.get_bogies().front(); - front = max(front, bogie.position+bogie.axles.front().position); - } - track_pos.advance(-front); - } + track_pos.advance(-type.get_front_axle_offset()); else if(m==FRONT_BUFFER) track_pos.advance(-type.get_length()/2); else if(m==BACK_AXLE) - { - float back = type.get_length()/2; - if(!type.get_axles().empty()) - back = type.get_axles().back().position; - if(!type.get_bogies().empty()) - { - const VehicleType::Bogie &bogie = type.get_bogies().back(); - back = min(back, bogie.position+bogie.axles.back().position); - } - track_pos.advance(-back); - } + track_pos.advance(-type.get_back_axle_offset()); else if(m==BACK_BUFFER) track_pos.advance(type.get_length()/2); update_position(); + propagate_position(); +} + +void Vehicle::unplace() +{ + if(!track_pos.track) + return; + + track_pos = TrackPosition(); + + if(prev) + prev->unplace(); + if(next) + next->unplace(); } void Vehicle::advance(float d) { track_pos.advance(d); update_position(); + propagate_position(); +} + +float Vehicle::get_bogie_direction(unsigned i) const +{ + if(i>=bogie_dirs.size()) + throw InvalidParameterValue("Bogie index out of range"); + return bogie_dirs[i]; } void Vehicle::update_position() @@ -79,58 +133,176 @@ void Vehicle::update_position() TrackPoint tp; const vector &axles = type.get_axles(); + const vector &bogies = type.get_bogies(); if(axles.size()>=2) - tp = get_position(axles.front().position, axles.back().position, track_pos); - else { - const vector &bogies = type.get_bogies(); - if(bogies.size()>=2) - // XXX Calculate bogie positions correctly - tp = get_position(bogies.front().position, bogies.back().position, track_pos); - else - tp = track_pos.get_point(); + float wheelbase = axles.front().position-axles.back().position; + tp = get_point(track_pos, wheelbase, -axles.back().position/wheelbase); } + else if(bogies.size()>=2) + { + TrackPosition front = track_pos; + front.advance(bogies.front().position); + TrackPosition back = track_pos; + back.advance(bogies.back().position); + float bogie_spacing = bogies.front().position-bogies.back().position; + adjust_for_distance(front, back, bogie_spacing); + + const vector &front_axles = bogies.front().axles; + float wheelbase = front_axles.front().position-front_axles.back().position; + TrackPoint front_point = get_point(front, wheelbase, -front_axles.back().position/wheelbase); + + const vector &back_axles = bogies.back().axles; + wheelbase = back_axles.front().position-back_axles.back().position; + TrackPoint back_point = get_point(back, wheelbase, -back_axles.back().position/wheelbase); + + tp = get_point(front_point.pos, back_point.pos, -bogies.back().position/bogie_spacing); + + bogie_dirs.front() = front_point.dir-tp.dir; + bogie_dirs.back() = back_point.dir-tp.dir; + } + else + tp = track_pos.get_point(); + + if(!prev) + check_sensor(type.get_front_axle_offset(), front_sensor); + if(!next) + check_sensor(type.get_back_axle_offset(), back_sensor); + position = tp.pos; + position.z += layout.get_catalogue().get_rail_elevation(); direction = tp.dir; } -TrackPoint Vehicle::get_position(float front, float back, const TrackPosition &tpos) +void Vehicle::update_position_from(const Vehicle &veh) { - TrackPosition front_pos = tpos; - front_pos.advance(front); + int sign = (&veh==prev ? -1 : 1); - TrackPosition back_pos = tpos; - back_pos.advance(back); + float tdist = (type.get_length()+veh.type.get_length())/2; + float margin = layout.get_catalogue().get_scale(); - float target_dist = front-back; + float dist = distance(veh.position, position); + if(disttdist+margin) + { + track_pos = veh.track_pos; + track_pos.advance(sign*tdist); + update_position(); + + dist = distance(veh.position, position); + } + + track_pos.advance(sign*(tdist-dist)); + update_position(); +} +void Vehicle::propagate_position() +{ + if(prev) + propagate_forward(); + if(next) + propagate_backward(); +} + +void Vehicle::propagate_forward() +{ + prev->update_position_from(*this); + + if(prev->prev) + prev->propagate_forward(); +} + +void Vehicle::propagate_backward() +{ + next->update_position_from(*this); + + if(next->next) + next->propagate_backward(); +} + +void Vehicle::check_sensor(float offset, unsigned &sensor) +{ + TrackPosition pos = track_pos; + pos.advance(offset); + unsigned s = pos.track->get_sensor_id(); + if(s!=sensor) + { + /* Sensor ID under axle has changed. Deduce movement direction by using + the sensor ID under the midpoint of the vehicle. */ + /* XXX This depends on the simulation running fast enough. Something + more robust would be preferable. */ + unsigned old = sensor; + sensor = s; + unsigned mid = track_pos.track->get_sensor_id(); + + if(s && s!=mid) + /* There's a sensor and it's different from mid. We've just entered + that sensor. */ + layout.get_driver().set_sensor(sensor, true); + if(old && old!=mid) + /* A sensor was under the axle and it was different from mid. We've + just left that sensor. */ + layout.get_driver().set_sensor(old, false); + } +} + +void Vehicle::adjust_for_distance(TrackPosition &front, TrackPosition &back, float tdist, float ratio) const +{ + float margin = 0.01*layout.get_catalogue().get_scale(); + int adjust_dir = 0; while(1) { - Point front_point = front_pos.get_point().pos; - Point back_point = back_pos.get_point().pos; + Point front_point = front.get_point().pos; + Point back_point = back.get_point().pos; float dx = front_point.x-back_point.x; float dy = front_point.y-back_point.y; float dz = front_point.z-back_point.z; float dist = sqrt(dx*dx+dy*dy+dz*dz); - if(distmargin && adjust_dir>=0) { - float f = -back/target_dist; - TrackPoint pt; - pt.pos = Point(back_point.x+dx*f, back_point.y+dy*f, back_point.z+dz*f); - pt.dir = atan2(dy, dx); - return pt; + diff += margin; + adjust_dir = 1; } + else + return; + + front.advance(diff*(1-ratio)); + back.advance(-diff*ratio); } } +TrackPoint Vehicle::get_point(const Point &front, const Point &back, float ratio) const +{ + float dx = front.x-back.x; + float dy = front.y-back.y; + float dz = front.z-back.z; + + TrackPoint tp; + tp.pos = Point(back.x+dx*ratio, back.y+dy*ratio, back.z+dz*ratio); + tp.dir = atan2(dy, dx); + + return tp; +} + +TrackPoint Vehicle::get_point(const TrackPosition &pos, float tdist, float ratio) const +{ + TrackPosition front = pos; + front.advance(tdist*(1-ratio)); + + TrackPosition back = pos; + back.advance(-tdist*ratio); + + adjust_for_distance(front, back, tdist, ratio); + return get_point(front.get_point().pos, back.get_point().pos, ratio); +} + Vehicle::TrackPosition::TrackPosition(): track(0), @@ -149,51 +321,36 @@ void Vehicle::TrackPosition::advance(float d) if(!track) return; - unsigned path = track->get_active_path(); - offs += d; - float path_len = track->get_type().get_path_length(path); - while(track && offs>=path_len) + TrackIter iter(track, ep); + while(iter) { - unsigned out = track->traverse(ep, path); - Track *next = track->get_link(out); + float path_len = iter->get_type().get_path_length(iter->get_active_path()); - if(next) + if(offs>path_len) { - ep = next->get_endpoint_by_link(*track); - track = next; - offs -= path_len; - path = track->get_active_path(); - path_len = track->get_type().get_path_length(path); + iter = iter.next(); } else - track = 0; + break; } - while(track && offs<0) + while(iter && offs<0) { - Track *prev = track->get_link(ep); - if(prev) - { - unsigned in = prev->get_endpoint_by_link(*track); - track = prev; + iter = iter.flip().reverse(); - path = track->get_active_path(); - ep = track->traverse(in, path); - - path_len = track->get_type().get_path_length(path); + if(iter) + { + float path_len = iter->get_type().get_path_length(iter->get_active_path()); offs += path_len; } - else - track = 0; } + track = iter.track(); + ep = iter.entry(); if(!track) - { - ep = 0; offs = 0; - } } TrackPoint Vehicle::TrackPosition::get_point() const