X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flibmarklin%2Fvehicle.cpp;fp=source%2Flibmarklin%2Fvehicle.cpp;h=7c299c78bd4311f6bb3b41506529a75fb99f4cce;hb=e5cd9e4fbc577036a0385c985b6b65df8218d0a2;hp=0000000000000000000000000000000000000000;hpb=9b68c21ebf8ce26d92246ecd4a392d2908b511f7;p=r2c2.git

diff --git a/source/libmarklin/vehicle.cpp b/source/libmarklin/vehicle.cpp
new file mode 100644
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