#include <cmath>
#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 {
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)
+ 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()
TrackPoint tp;
const vector<VehicleType::Axle> &axles = type.get_axles();
+ const vector<VehicleType::Bogie> &bogies = type.get_bogies();
if(axles.size()>=2)
- tp = get_position(axles.front().position, axles.back().position, track_pos);
- else
{
- const vector<VehicleType::Bogie> &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<VehicleType::Axle> &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<VehicleType::Axle> &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)
+{
+ 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(dist<tdist-margin || dist>tdist+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();
+}
- TrackPosition back_pos = tpos;
- back_pos.advance(back);
+void Vehicle::propagate_forward()
+{
+ prev->update_position_from(*this);
- float target_dist = front-back;
+ 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(dist<target_dist)
+ float diff = tdist-dist;
+ if(diff<-margin && adjust_dir<=0)
{
- float adjust = target_dist-dist+0.01*layout.get_catalogue().get_scale();
- front_pos.advance(adjust/2);
- back_pos.advance(-adjust/2);
+ diff -= margin;
+ adjust_dir = -1;
}
- else
+ else if(diff>margin && 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),
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