Add vehicles

[r2c2.git] / source / libmarklin / vehicle.cpp

diff --git a/source/libmarklin/vehicle.cpp b/source/libmarklin/vehicle.cpp
new file mode 100644 (file)
index 0000000..7c299c7
--- /dev/null
+++ b/source/libmarklin/vehicle.cpp
@@ -0,0 +1,189 @@
+/* $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