+/* $Id$
+
+This file is part of the MSP Märklin suite
+Copyright © 2010 Mikkosoft Productions, Mikko Rasa
+Distributed under the GPL
+*/
+
+#include <cmath>
+#include "catalogue.h"
+#include "layout.h"
+#include "track.h"
+#include "tracktype.h"
+#include "vehicle.h"
+#include "vehicletype.h"
+
+using namespace std;
+
+namespace Marklin {
+
+Vehicle::Vehicle(Layout &l, const VehicleType &t):
+ layout(l),
+ type(t),
+ next(0),
+ prev(0),
+ direction(0)
+{
+ layout.add_vehicle(*this);
+}
+
+Vehicle::~Vehicle()
+{
+ layout.remove_vehicle(*this);
+}
+
+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);
+ }
+ update_position();
+}
+
+void Vehicle::advance(float d)
+{
+ track_pos.advance(d);
+ update_position();
+}
+
+void Vehicle::update_position()
+{
+ TrackPoint tp;
+
+ const vector<VehicleType::Axle> &axles = type.get_axles();
+ 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();
+ }
+ position = tp.pos;
+ direction = tp.dir;
+}
+
+TrackPoint Vehicle::get_position(float front, float back, const TrackPosition &tpos)
+{
+ TrackPosition front_pos = tpos;
+ front_pos.advance(front);
+
+ TrackPosition back_pos = tpos;
+ back_pos.advance(back);
+
+ float target_dist = front-back;
+
+ while(1)
+ {
+ Point front_point = front_pos.get_point().pos;
+ Point back_point = back_pos.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 adjust = target_dist-dist+0.01*layout.get_catalogue().get_scale();
+ front_pos.advance(adjust/2);
+ back_pos.advance(-adjust/2);
+ }
+ else
+ {
+ 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;
+ }
+ }
+}
+
+
+Vehicle::TrackPosition::TrackPosition():
+ track(0),
+ ep(0),
+ offs(0)
+{ }
+
+Vehicle::TrackPosition::TrackPosition(Track *t, unsigned e, float o):
+ track(t),
+ ep(e),
+ offs(o)
+{ }
+
+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)
+ {
+ unsigned out = track->traverse(ep, path);
+ Track *next = track->get_link(out);
+
+ if(next)
+ {
+ 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);
+ }
+ else
+ track = 0;
+ }
+
+ while(track && offs<0)
+ {
+ Track *prev = track->get_link(ep);
+ if(prev)
+ {
+ unsigned in = prev->get_endpoint_by_link(*track);
+ track = prev;
+
+ path = track->get_active_path();
+ ep = track->traverse(in, path);
+
+ path_len = track->get_type().get_path_length(path);
+ offs += path_len;
+ }
+ else
+ track = 0;
+ }
+
+ if(!track)
+ {
+ ep = 0;
+ offs = 0;
+ }
+}
+
+TrackPoint Vehicle::TrackPosition::get_point() const
+{
+ if(track)
+ return track->get_point(ep, offs);
+ else
+ return TrackPoint();
+}
+
+} // namespace Marklin