X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flibmarklin%2Fvehicle.cpp;h=1f3d3ec178e56a771987264a495b5c20982d80b9;hb=a4566a4a23ff676f11a90615e8e468ef50107946;hp=6011537d02b3205a4996a89a80edad272cfe2a2e;hpb=e94014530a6e28a42bc5678e579ee07e0ee5329b;p=r2c2.git diff --git a/source/libmarklin/vehicle.cpp b/source/libmarklin/vehicle.cpp index 6011537..1f3d3ec 100644 --- a/source/libmarklin/vehicle.cpp +++ b/source/libmarklin/vehicle.cpp @@ -14,6 +14,7 @@ Distributed under the GPL #include "vehicletype.h" using namespace std; +using namespace Msp; namespace Marklin { @@ -22,7 +23,8 @@ Vehicle::Vehicle(Layout &l, const VehicleType &t): type(t), next(0), prev(0), - direction(0) + direction(0), + bogie_dirs(type.get_bogies().size()) { layout.add_vehicle(*this); } @@ -32,6 +34,42 @@ Vehicle::~Vehicle() layout.remove_vehicle(*this); } +void Vehicle::attach_back(Vehicle &veh) +{ + if(next || veh.prev) + throw InvalidState("Already attached"); + + next = &veh; + veh.prev = this; +} + +void Vehicle::attach_front(Vehicle &veh) +{ + if(prev || veh.next) + throw InvalidState("Already attached"); + + next = &veh; + veh.prev = this; +} + +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); @@ -66,12 +104,21 @@ void Vehicle::place(Track *t, unsigned e, float o, PlaceMode m) track_pos.advance(type.get_length()/2); update_position(); + propagate_position(); } 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() @@ -80,57 +127,146 @@ void Vehicle::update_position() const vector &axles = type.get_axles(); if(axles.size()>=2) - tp = get_position(axles.front().position, axles.back().position, track_pos); + { + float wheelbase = axles.front().position-axles.back().position; + tp = get_point(track_pos, wheelbase, -axles.back().position/wheelbase); + } 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); + { + 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(); } + position = tp.pos; direction = tp.dir; } -TrackPoint Vehicle::get_position(float front, float back, const TrackPosition &tpos) +void Vehicle::update_position_from(const Vehicle &veh) +{ + int sign = (&veh==prev ? -1 : 1); + + float tdist = (type.get_length()+veh.type.get_length())/2; + float margin = layout.get_catalogue().get_scale(); + + 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() { - TrackPosition front_pos = tpos; - front_pos.advance(front); + if(prev) + propagate_forward(); + if(next) + propagate_backward(); +} + +void Vehicle::propagate_forward() +{ + prev->update_position_from(*this); - TrackPosition back_pos = tpos; - back_pos.advance(back); + if(prev->prev) + prev->propagate_forward(); +} - float target_dist = front-back; +void Vehicle::propagate_backward() +{ + next->update_position_from(*this); + + if(next->next) + next->propagate_backward(); +} +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),