#include "vehicletype.h"
using namespace std;
+using namespace Msp;
namespace Marklin {
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);
track_pos.advance(type.get_length()/2);
update_position();
+ propagate_position();
}
void Vehicle::advance(float d)
{
track_pos.advance(d);
update_position();
+ propagate_position();
}
void Vehicle::update_position()
const vector<VehicleType::Axle> &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<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);
+ {
+ 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;
+ Point front_point = get_point(front, wheelbase, -front_axles.back().position/wheelbase).pos;
+
+ const vector<VehicleType::Axle> &back_axles = bogies.back().axles;
+ wheelbase = back_axles.front().position-back_axles.back().position;
+ Point back_point = get_point(back, wheelbase, -back_axles.back().position/wheelbase).pos;
+
+ tp = get_point(front_point, back_point, -bogies.back().position/bogie_spacing);
+ }
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)
{
- 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(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()
+{
+ 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::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),
projects / r2c2.git / blobdiff