From 7a36d396eded897c421424905b2c938d770df341 Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Sun, 19 May 2013 15:31:15 +0300
Subject: [PATCH] Make use of the mspmath library

---
 Build                                |  1 +
 source/3d/axle.cpp                   |  3 +-
 source/3d/bogie.cpp                  |  7 ++-
 source/3d/endpoint.cpp               |  4 +-
 source/3d/layout.cpp                 |  2 +-
 source/3d/overlay.cpp                |  3 +-
 source/3d/rod.cpp                    |  5 +-
 source/3d/signal.cpp                 |  3 +-
 source/3d/track.cpp                  | 31 +++---------
 source/3d/track.h                    |  2 +-
 source/3d/tracktype.cpp              | 46 ++++++++---------
 source/3d/tracktype.h                |  2 +-
 source/3d/vehicle.cpp                |  7 ++-
 source/3d/view.cpp                   |  4 +-
 source/designer/cameracontroller.cpp | 23 ++++-----
 source/designer/cameracontroller.h   |  4 +-
 source/designer/designer.cpp         | 10 ++--
 source/designer/manipulator.cpp      | 76 +++++++++++-----------------
 source/designer/manipulator.h        |  9 ++--
 source/designer/measure.cpp          | 18 +++----
 source/designer/measure.h            |  5 +-
 source/designer/svgexporter.cpp      | 63 ++++++++++-------------
 source/designer/trackwrap.cpp        | 13 ++---
 source/engineer/engineer.cpp         |  6 +--
 source/engineer/trainview.cpp        | 20 ++++----
 source/libr2c2/catalogue.cpp         |  4 +-
 source/libr2c2/centralstation.cpp    |  4 +-
 source/libr2c2/geometry.h            | 28 ++++++----
 source/libr2c2/layout.h              |  2 +-
 source/libr2c2/object.cpp            | 10 ++--
 source/libr2c2/object.h              |  6 +--
 source/libr2c2/profile.cpp           | 20 +++-----
 source/libr2c2/signal.cpp            | 43 ++++++----------
 source/libr2c2/signal.h              |  2 +-
 source/libr2c2/snap.h                |  4 +-
 source/libr2c2/track.cpp             | 52 +++++++------------
 source/libr2c2/track.h               |  2 +-
 source/libr2c2/trackpart.cpp         | 70 ++++++++++---------------
 source/libr2c2/trackpart.h           |  2 +-
 source/libr2c2/tracktype.cpp         | 24 ++++-----
 source/libr2c2/tracktype.h           |  4 +-
 source/libr2c2/vehicle.cpp           | 71 +++++++++++---------------
 source/libr2c2/vehicle.h             |  8 +--
 43 files changed, 301 insertions(+), 422 deletions(-)

diff --git a/Build b/Build
index 11a61c7..eed4769 100644
--- a/Build
+++ b/Build
@@ -2,6 +2,7 @@ package "r2c2"
 {
 	require "mspcore";
 	require "mspdatafile";
+	require "mspmath";
 	require "sigc++-2.0";
 
 	library "r2c2"
diff --git a/source/3d/axle.cpp b/source/3d/axle.cpp
index c9f7a70..e52303a 100644
--- a/source/3d/axle.cpp
+++ b/source/3d/axle.cpp
@@ -34,8 +34,7 @@ void Axle3D::setup_render(GL::Renderer &renderer, const GL::Tag &) const
 {
 	GL::Matrix matrix;
 
-	const Vector &pos = vehicle.get_position();
-	matrix.translate(pos.x, pos.y, pos.z);
+	matrix.translate(vehicle.get_position());
 	matrix.rotate(vehicle.get_rotation(), 0, 0, 1);
 
 	if(bogie)
diff --git a/source/3d/bogie.cpp b/source/3d/bogie.cpp
index 8e99ceb..e00e373 100644
--- a/source/3d/bogie.cpp
+++ b/source/3d/bogie.cpp
@@ -26,14 +26,13 @@ void Bogie3D::setup_render(Msp::GL::Renderer &renderer, const GL::Tag &) const
 {
 	GL::Matrix matrix;
 
-	const Vector &pos = vehicle.get_position();
-	matrix.translate(pos.x, pos.y, pos.z);
+	matrix.translate(vehicle.get_position());
 	matrix.rotate(vehicle.get_rotation(), 0, 0, 1);
 
 	matrix.translate(bogie.type->position, 0, 0);
-	float dir = bogie.direction;
+	Angle dir = bogie.direction;
 	if(bogie.type->rotate_object)
-		dir += M_PI;
+		dir += Angle::half_turn();
 	matrix.rotate(dir, 0, 0, 1);
 
 	renderer.matrix_stack() *= matrix;
diff --git a/source/3d/endpoint.cpp b/source/3d/endpoint.cpp
index 5d7ebb5..97e05a4 100644
--- a/source/3d/endpoint.cpp
+++ b/source/3d/endpoint.cpp
@@ -31,8 +31,8 @@ void Endpoint3D::render(GL::Renderer &renderer, const GL::Tag &tag) const
 
 		GL::MatrixStack::Push push_mtx(renderer.matrix_stack());
 		GL::Matrix matrix;
-		matrix.translate(sn.position.x, sn.position.y, sn.position.z);
-		matrix.rotate(sn.rotation+M_PI, 0, 0, 1);
+		matrix.translate(sn.position);
+		matrix.rotate(sn.rotation+Angle::half_turn(), 0, 0, 1);
 		renderer.matrix_stack() *= matrix;
 
 		if(track.get_track().get_link(index))
diff --git a/source/3d/layout.cpp b/source/3d/layout.cpp
index 6042515..d97dfe5 100644
--- a/source/3d/layout.cpp
+++ b/source/3d/layout.cpp
@@ -44,7 +44,7 @@ void Layout3D::get_bounds(Vector &minp, Vector &maxp) const
 	{
 		Vector tmin;
 		Vector tmax;
-		i->second->get_bounds(0, tmin, tmax);
+		i->second->get_bounds(Angle::zero(), tmin, tmax);
 		minp.x = min(minp.x, tmin.x);
 		minp.y = min(minp.y, tmin.y);
 		maxp.x = max(maxp.x, tmax.x);
diff --git a/source/3d/overlay.cpp b/source/3d/overlay.cpp
index bab469c..a9083e5 100644
--- a/source/3d/overlay.cpp
+++ b/source/3d/overlay.cpp
@@ -95,8 +95,7 @@ void Overlay3D::render(GL::Renderer &renderer, const GL::Tag &) const
 
 		const Icon &icon = *i->second;
 
-		Vector node = i->first->get_node();
-		GL::Vector3 p = renderer.get_camera()->project(GL::Vector3(node.x, node.y, node.z));
+		GL::Vector3 p = renderer.get_camera()->project(i->first->get_node());
 		p.x = int(p.x*0.5*fb_width-icon.width*size/2);
 		p.y = int(p.y*0.5*fb_height);
 
diff --git a/source/3d/rod.cpp b/source/3d/rod.cpp
index 10b5f46..5ae4584 100644
--- a/source/3d/rod.cpp
+++ b/source/3d/rod.cpp
@@ -25,11 +25,10 @@ void Rod3D::render(GL::Renderer &renderer, const GL::Tag &tag) const
 void Rod3D::setup_render(GL::Renderer &renderer, const GL::Tag &) const
 {
 	GL::Matrix matrix;
-	const Vector &pos = vehicle.get_position();
-	matrix.translate(pos.x, pos.y, pos.z);
+	matrix.translate(vehicle.get_position());
 	matrix.rotate(vehicle.get_rotation(), 0, 0, 1);
 
-	matrix.translate(rod.position.x, rod.position.y, rod.position.z);
+	matrix.translate(rod.position);
 	if(rod.type->mirror_object)
 		matrix.scale(1, -1, 1);
 	matrix.rotate(rod.angle, 0, -1, 0);
diff --git a/source/3d/signal.cpp b/source/3d/signal.cpp
index 0942bce..4f12467 100644
--- a/source/3d/signal.cpp
+++ b/source/3d/signal.cpp
@@ -25,8 +25,7 @@ Signal3D::~Signal3D()
 
 void Signal3D::setup_render(GL::Renderer &renderer, const GL::Tag &) const
 {
-	const Vector &pos = signal.get_position();
-	renderer.matrix_stack() *= GL::Matrix::translation(pos.x, pos.y, pos.z);
+	renderer.matrix_stack() *= GL::Matrix::translation(signal.get_position());
 	renderer.matrix_stack() *= GL::Matrix::rotation(signal.get_rotation(), 0, 0, 1);
 	// XXX Use track gauge, configure signal side
 	renderer.matrix_stack() *= GL::Matrix::translation(0, -0.035, 0);
diff --git a/source/3d/track.cpp b/source/3d/track.cpp
index 72973f3..bd148a7 100644
--- a/source/3d/track.cpp
+++ b/source/3d/track.cpp
@@ -47,20 +47,13 @@ Track3D::~Track3D()
 		delete *i;
 }
 
-void Track3D::get_bounds(float angle, Vector &minp, Vector &maxp) const
+void Track3D::get_bounds(const Angle &angle, Vector &minp, Vector &maxp) const
 {
 	type.get_bounds(angle-track.get_rotation(), minp, maxp);
 
-	float c = cos(-angle);
-	float s = sin(-angle);
-
-	const Vector &pos = track.get_position();
-	minp.x += c*pos.x-s*pos.y;
-	maxp.x += c*pos.x-s*pos.y;
-	minp.y += s*pos.x+c*pos.y;
-	maxp.y += s*pos.x+c*pos.y;
-	minp.z += pos.z;
-	maxp.z += pos.z;
+	Vector pos = rotated_vector(track.get_position(), -angle);
+	minp += pos;
+	maxp += pos;
 
 	float slope = track.get_slope();
 	if(slope>0)
@@ -71,26 +64,18 @@ void Track3D::get_bounds(float angle, Vector &minp, Vector &maxp) const
 
 Vector Track3D::get_node() const
 {
-	const Vector &pos = track.get_position();
 	Vector minp;
 	Vector maxp;
-	type.get_bounds(0, minp, maxp);
-	float rot = track.get_rotation();
-	float c = cos(rot);
-	float s = sin(rot);
+	type.get_bounds(Angle::zero(), minp, maxp);
 
-	Vector center((minp.x+maxp.x)/2, (minp.y+maxp.y)/2, 0);
-	return Vector(pos.x+c*center.x-s*center.y, pos.y+s*center.x+c*center.y, pos.z+0.02);
+	return track.get_position()+rotated_vector((minp+maxp)/2.0f, track.get_rotation())+Vector(0, 0, 0.02);
 }
 
 GL::Matrix Track3D::create_matrix() const
 {
-	const Vector &pos = track.get_position();
-	float rot = track.get_rotation();
-
 	GL::Matrix matrix;
-	matrix.translate(pos.x, pos.y, pos.z);
-	matrix.rotate(rot, 0, 0, 1);
+	matrix.translate(track.get_position());
+	matrix.rotate(track.get_rotation(), 0, 0, 1);
 	matrix.rotate(track.get_slope()/track.get_type().get_total_length(), 0, -1, 0);
 
 	return matrix;
diff --git a/source/3d/track.h b/source/3d/track.h
index f3bfb45..641f234 100644
--- a/source/3d/track.h
+++ b/source/3d/track.h
@@ -32,7 +32,7 @@ public:
 	Layout3D &get_layout() const { return layout; }
 	Track &get_track() const { return track; }
 	const TrackType3D &get_type() const { return type; }
-	void get_bounds(float, Vector &, Vector &) const;
+	void get_bounds(const Angle &, Vector &, Vector &) const;
 	Path3D &get_path() { return *path; }
 
 	virtual Vector get_node() const;
diff --git a/source/3d/tracktype.cpp b/source/3d/tracktype.cpp
index ab417cf..a5e97cc 100644
--- a/source/3d/tracktype.cpp
+++ b/source/3d/tracktype.cpp
@@ -33,7 +33,7 @@ Iter graham_scan(Iter begin, Iter end)
 	for(Iter k=begin, i=k++, j=k++;; )
 	{
 		// Compute winding by cross product
-		float turn = (j->x-i->x)*(k->y-j->y) - (k->x-j->x)*(j->y-i->y);
+		float turn = cross(*j-*i, *k-*j).z;
 
 		if(turn<1e-5)
 		{
@@ -96,7 +96,7 @@ TrackType3D::TrackType3D(Catalogue3D &cat3d, const TrackType &tt):
 			for(unsigned i=0; i<vertices.size(); ++i)
 			{
 				const float *v = vertices[i]+vertex_offs;
-				border.push_back(Vector(v[0], v[1]));
+				border.push_back(Vector(v[0], v[1], 0));
 			}
 		}
 	}
@@ -109,14 +109,14 @@ TrackType3D::TrackType3D(Catalogue3D &cat3d, const TrackType &tt):
 		unsigned index = 0;
 		bld.texcoord(0.25, 0.5);
 		for(vector<TrackPart>::const_iterator i=parts.begin(); i!=parts.end(); ++i)
-			build_part(*i, ballast_profile, Vector(0, -ballast_min.y), false, bld, index);
+			build_part(*i, ballast_profile, Vector(0, -ballast_min.y, 0), false, bld, index);
 
 		bld.texcoord(0.75, 0.5);
 		float y = ballast_h-rail_min.y;
 		for(vector<TrackPart>::const_iterator i=parts.begin(); i!=parts.end(); ++i)
-			build_part(*i, rail_profile, Vector(-gauge/2, y), true, bld, index);
+			build_part(*i, rail_profile, Vector(0, gauge/2, y), true, bld, index);
 		for(vector<TrackPart>::const_iterator i=parts.begin(); i!=parts.end(); ++i)
-			build_part(*i, rail_profile, Vector(gauge/2, y), false, bld, index);
+			build_part(*i, rail_profile, Vector(0, -gauge/2, y), false, bld, index);
 
 		object = new GL::Object;
 		object->set_mesh(mesh);
@@ -134,7 +134,7 @@ TrackType3D::TrackType3D(Catalogue3D &cat3d, const TrackType &tt):
 			unsigned index = 0;
 			for(vector<TrackPart>::const_iterator j=parts.begin(); j!=parts.end(); ++j)
 				if(j->get_path()==i)
-					build_part(*j, cat.get_path_profile(), Vector(0, ballast_h+1.5*rail_h), false, bld, index);
+					build_part(*j, cat.get_path_profile(), Vector(0, 0, ballast_h+1.5*rail_h), false, bld, index);
 		}
 		path_meshes.push_back(m);
 	}
@@ -154,10 +154,9 @@ TrackType3D::~TrackType3D()
 		delete *i;
 }
 
-void TrackType3D::get_bounds(float angle, Vector &minp, Vector &maxp) const
+void TrackType3D::get_bounds(const Angle &angle, Vector &minp, Vector &maxp) const
 {
-	float c = cos(-angle);
-	float s = sin(-angle);
+	Transform trans = Transform::rotation(-angle, Vector(0, 0, 1));
 
 	minp = maxp = Vector();
 	minp.z = min_z;
@@ -165,13 +164,12 @@ void TrackType3D::get_bounds(float angle, Vector &minp, Vector &maxp) const
 
 	for(vector<Vector>::const_iterator i=border.begin(); i!=border.end(); ++i)
 	{
-		float x = c*i->x-s*i->y;
-		float y = s*i->x+c*i->y;
+		Vector v = trans.transform(*i);
 
-		minp.x = min(minp.x, x);
-		minp.y = min(minp.y, y);
-		maxp.x = max(maxp.x, x);
-		maxp.y = max(maxp.y, y);
+		minp.x = min(minp.x, v.x);
+		minp.y = min(minp.y, v.y);
+		maxp.x = max(maxp.x, v.x);
+		maxp.y = max(maxp.y, v.y);
 	}
 }
 
@@ -193,24 +191,22 @@ void TrackType3D::build_part(const TrackPart &part, const Profile &profile, cons
 	for(unsigned i=0; i<=nsegs; ++i)
 	{
 		TrackPoint basep = part.get_point(i*plen/nsegs);
-		float c = cos(basep.dir);
-		float s = sin(basep.dir);
+		Transform trans = Transform::rotation(basep.dir, Vector(0, 0, 1));
 
 		for(unsigned j=0; j<n_vertices; ++j)
 		{
 			const Profile::Vertex &v = profile.get_vertex(mirror ? n_vertices-1-j : j);
-			Vector p = v.pos;
+			Vector p(0, -v.pos.x, v.pos.y);
 			if(mirror)
-				p.x = -p.x;
-			p.z = basep.pos.z+p.y+offset.y;
-			p.y = basep.pos.y-c*(p.x+offset.x);
-			p.x = basep.pos.x+s*(p.x+offset.x);
+				p.y = -p.y;
+			p = basep.pos+trans.transform(offset+p);
 
-			Vector n = v.normal;
+			Vector n(0, -v.normal.x, v.normal.y);
 			if(mirror)
-				n.x = -n.x;
+				n.y = -n.y;
+			n = trans.transform(n);
 
-			bld.normal(s*n.x, -c*n.x, n.y);
+			bld.normal(n.x, n.y, n.z);
 			bld.vertex(p.x, p.y, p.z);
 
 			border.push_back(p);
diff --git a/source/3d/tracktype.h b/source/3d/tracktype.h
index a0f4e4e..0f6b30f 100644
--- a/source/3d/tracktype.h
+++ b/source/3d/tracktype.h
@@ -26,7 +26,7 @@ public:
 	TrackType3D(Catalogue3D &, const TrackType &);
 	~TrackType3D();
 
-	void get_bounds(float, Vector &, Vector &) const;
+	void get_bounds(const Angle &, Vector &, Vector &) const;
 	const Msp::GL::Object &get_object() const { return *object; }
 	const Msp::GL::Mesh &get_path_mesh(unsigned) const;
 
diff --git a/source/3d/vehicle.cpp b/source/3d/vehicle.cpp
index d1e5657..38e2fb5 100644
--- a/source/3d/vehicle.cpp
+++ b/source/3d/vehicle.cpp
@@ -93,11 +93,10 @@ void Vehicle3D::render(GL::Renderer &renderer, const GL::Tag &tag) const
 void Vehicle3D::setup_render(GL::Renderer &renderer, const GL::Tag &) const
 {
 	GL::Matrix matrix;
-	const Vector &pos = vehicle.get_position();
-	matrix.translate(pos.x, pos.y, pos.z);
-	float rot = vehicle.get_rotation();
+	matrix.translate(vehicle.get_position());
+	Angle rot = vehicle.get_rotation();
 	if(vehicle.get_type().get_rotate_object())
-		rot += M_PI;
+		rot += Angle::half_turn();
 	matrix.rotate(rot, 0, 0, 1);
 	renderer.matrix_stack() *= matrix;
 }
diff --git a/source/3d/view.cpp b/source/3d/view.cpp
index 4cd553d..8b63553 100644
--- a/source/3d/view.cpp
+++ b/source/3d/view.cpp
@@ -38,9 +38,9 @@ void View3D::view_all(bool tight)
 	float dist = size/t;
 	if(!tight)
 		dist += sin(camera.get_field_of_view()/2)*size;
-	GL::Vector3 center((minp.x+maxp.x)/2, (minp.y+maxp.y)/2, 0);
+	GL::Vector3 center = (minp+maxp)/2.0f;
 	const GL::Vector3 &look = camera.get_look_direction();
-	camera.set_position(GL::Vector3(center.x-look.x*dist, center.y-look.y*dist, center.z-look.z*dist));
+	camera.set_position(center-look*dist);
 }
 
 void View3D::render()
diff --git a/source/designer/cameracontroller.cpp b/source/designer/cameracontroller.cpp
index f8d1a53..0679cbe 100644
--- a/source/designer/cameracontroller.cpp
+++ b/source/designer/cameracontroller.cpp
@@ -37,7 +37,7 @@ void CameraController::set_look_direction(const GL::Vector3 &look)
 	GL::Vector3 focus = get_focus();
 	float dist = get_distance();
 	camera.set_look_direction(look);
-	camera.set_position(GL::Vector3(focus.x-look.x*dist, focus.y-look.y*dist, focus.z-look.z*dist));
+	camera.set_position(focus-look*dist);
 }
 
 void CameraController::view_all()
@@ -49,10 +49,9 @@ void CameraController::move(float x, float y)
 {
 	const GL::Vector3 &pos = camera.get_position();
 	const GL::Vector3 &look = camera.get_look_direction();
-	float xy_len = sqrt(look.x*look.x+look.y*look.y);
-	float dx = (look.x*y+look.y*x)/xy_len;
-	float dy = (look.y*y-look.x*x)/xy_len;
-	camera.set_position(GL::Vector3(pos.x+dx, pos.y+dy, pos.z));
+	GL::Vector3 fwd_dir = normalize(GL::Vector3(look.x, look.y, 0));
+	GL::Vector3 right_dir = cross(fwd_dir, GL::Vector3(0, 0, 1));
+	camera.set_position(pos+fwd_dir*y+right_dir*x);
 }
 
 void CameraController::adjust_distance(float delta)
@@ -63,20 +62,18 @@ void CameraController::adjust_distance(float delta)
 	float low = view.get_layout().get_layout().get_catalogue().get_gauge()*5;
 	if(dist+delta<low)
 		delta = low-dist;
-	camera.set_position(GL::Vector3(pos.x-look.x*delta, pos.y-look.y*delta, pos.z-look.z*delta));
+	camera.set_position(pos-look*delta);
 	dist += delta;
 	camera.set_depth_clip(dist*0.02, dist*50);
 }
 
-void CameraController::rotate(float angle)
+void CameraController::rotate(const Angle &angle)
 {
 	GL::Vector3 look = camera.get_look_direction();
-	float c = cos(angle);
-	float s = sin(angle);
-	set_look_direction(GL::Vector3(look.x*c-look.y*s, look.x*s+look.y*c, look.z));
+	set_look_direction(rotated_vector(look, angle));
 }
 
-void CameraController::pitch(float angle)
+void CameraController::pitch(const Angle &angle)
 {
 	GL::Vector3 look = camera.get_look_direction();
 	float xy_len = sqrt(look.x*look.x+look.y*look.y);
@@ -139,9 +136,9 @@ void CameraController::axis_motion(unsigned axis, float, float change)
 	else if(drag_mode==ROTATE)
 	{
 		if(axis==0)
-			rotate(-change*M_PI);
+			rotate(Angle::from_turns(-change/2));
 		else if(axis==1)
-			pitch(change*M_PI/2);
+			pitch(Angle::from_turns(change/4));
 	}
 	else if(drag_mode==DISTANCE && axis==1)
 		adjust_distance(-change*3*get_distance());
diff --git a/source/designer/cameracontroller.h b/source/designer/cameracontroller.h
index 4ed45b0..5b6c315 100644
--- a/source/designer/cameracontroller.h
+++ b/source/designer/cameracontroller.h
@@ -53,8 +53,8 @@ public:
 
 	void move(float, float);
 	void adjust_distance(float);
-	void rotate(float);
-	void pitch(float);
+	void rotate(const R2C2::Angle &);
+	void pitch(const R2C2::Angle &);
 
 	void tick(float);
 private:
diff --git a/source/designer/designer.cpp b/source/designer/designer.cpp
index edbec38..6d2fd41 100644
--- a/source/designer/designer.cpp
+++ b/source/designer/designer.cpp
@@ -332,7 +332,7 @@ Vector Designer::map_pointer_to_ground(const Vector &p)
 	GL::Vector4 vec = main_view->get_camera().unproject(GL::Vector4(p.x, p.y, 0, 0));
 	const GL::Vector3 &pos = main_view->get_camera().get_position();
 
-	return Vector(pos.x-vec.x*pos.z/vec.z, pos.y-vec.y*pos.z/vec.z);
+	return pos-GL::Vector3(vec.x, vec.y, vec.z)*(pos.z/vec.z);
 }
 
 void Designer::tick()
@@ -450,7 +450,7 @@ void Designer::button_press(unsigned btn)
 {
 	bool shift = keyboard.get_button_state(Input::KEY_SHIFT_L) || keyboard.get_button_state(Input::KEY_SHIFT_R);
 
-	Vector pointer(mouse.get_axis_value(0), mouse.get_axis_value(1));
+	Vector pointer(mouse.get_axis_value(0), mouse.get_axis_value(1), 0);
 	Vector ground = map_pointer_to_ground(pointer);
 
 	if(mode==CATALOGUE)
@@ -491,7 +491,7 @@ void Designer::button_press(unsigned btn)
 
 void Designer::axis_motion(unsigned, float, float)
 {
-	Vector pointer(mouse.get_axis_value(0), mouse.get_axis_value(1));
+	Vector pointer(mouse.get_axis_value(0), mouse.get_axis_value(1), 0);
 	float wx = pointer.x*window.get_width();
 	float wy = pointer.y*window.get_height();
 
@@ -587,8 +587,8 @@ void Designer::measure_changed()
 	float pard = measure.get_parallel_distance()*1000;
 	float perpd = measure.get_perpendicular_distance()*1000;
 	float d = sqrt(pard*pard+perpd*perpd);
-	float adiff = measure.get_angle_difference()*180/M_PI;
-	string info = format("Par %.1fmm - Perp %.1fmm - Total %.1fmm - Angle %.1f°", pard, perpd, d, adiff);
+	const Angle &adiff = measure.get_angle_difference();
+	string info = format("Par %.1fmm - Perp %.1fmm - Total %.1fmm - Angle %.1f°", pard, perpd, d, adiff.degrees());
 	lbl_status->set_text(info);
 }
 
diff --git a/source/designer/manipulator.cpp b/source/designer/manipulator.cpp
index 802adb1..8620fe7 100644
--- a/source/designer/manipulator.cpp
+++ b/source/designer/manipulator.cpp
@@ -14,8 +14,7 @@ Manipulator::Manipulator(Designer &d, Input::Mouse &m, Selection &s):
 	designer(d),
 	mouse(m),
 	selection(s),
-	mode(NONE),
-	angle(0)
+	mode(NONE)
 {
 	mouse.signal_button_press.connect(sigc::bind_return(sigc::mem_fun(this, &Manipulator::button_press), false));
 	mouse.signal_axis_motion.connect(sigc::bind_return(sigc::mem_fun(this, &Manipulator::axis_motion), false));
@@ -37,7 +36,7 @@ void Manipulator::start_rotate()
 	if(mode)
 		cancel();
 
-	rot_origin = atan2(gpointer.y-center.y, gpointer.x-center.x);
+	rot_origin = Geometry::atan2(gpointer.y-center.y, gpointer.x-center.x);
 
 	mode = ROTATE;
 }
@@ -243,22 +242,15 @@ void Manipulator::connect()
 			if(abs(dz)>0.02)
 				continue;
 
-			float adiff = sn1.rotation+M_PI-sn2.rotation;
-			while(adiff<-M_PI)
-				adiff += M_PI*2;
-			while(adiff>M_PI)
-				adiff -= M_PI*2;
-			if(abs(adiff)>0.01)
+			Angle adiff = wrap_balanced(sn1.rotation+Angle::half_turn()-sn2.rotation);
+			if(abs(adiff).radians()>0.01)
 				continue;
 
-			float c = cos(sn1.rotation);
-			float s = sin(sn1.rotation);
-			float dx = sn2.position.x-sn1.position.x;
-			float dy = sn2.position.y-sn1.position.y;
-			if(abs(dx*s-dy*c)>limit)
+			Vector delta = rotated_vector(sn2.position-sn1.position, -sn1.rotation);
+			if(abs(delta.y)>limit)
 				continue;
 
-			gap = dx*c+dy*s;
+			gap = delta.x;
 			if(gap<0)
 				continue;
 
@@ -297,7 +289,7 @@ void Manipulator::cancel()
 
 	for(vector<MObject>::iterator i=objects.begin(); i!=objects.end(); ++i)
 	{
-		i->object->set_position(Vector(center.x+i->pos.x, center.y+i->pos.y, center.z+i->pos.z));
+		i->object->set_position(center+i->pos);
 		i->object->set_rotation(i->rot);
 	}
 
@@ -305,7 +297,7 @@ void Manipulator::cancel()
 		delete *i;
 	extend_tracks.clear();
 
-	angle = 0;
+	angle = Angle::zero();
 
 	signal_done.emit(false);
 }
@@ -321,7 +313,7 @@ void Manipulator::button_press(unsigned btn)
 	{
 		Mode m = mode;
 		mode = NONE;
-		angle = 0;
+		angle = Angle::zero();
 
 		if(m!=EXTEND)
 		{
@@ -367,11 +359,10 @@ void Manipulator::axis_motion(unsigned axis, float value, float)
 
 	if(mode==MOVE)
 	{
-		Vector delta(gpointer.x-move_origin.x, gpointer.y-move_origin.y, 0);
-		Vector offset(center.x+delta.x, center.y+delta.y, center.z);
+		Vector offset = center+gpointer-move_origin;
 		for(vector<MObject>::iterator i=objects.begin(); i!=objects.end(); ++i)
 		{
-			i->object->set_position(Vector(offset.x+i->pos.x, offset.y+i->pos.y, offset.z+i->pos.z));
+			i->object->set_position(offset+i->pos);
 			i->object->set_rotation(i->rot);
 		}
 
@@ -394,32 +385,29 @@ void Manipulator::axis_motion(unsigned axis, float value, float)
 
 		if(snapped)
 		{
-			float da = snapped->object->get_rotation()-snapped->rot;
-			float c = cos(da);
-			float s = sin(da);
+			Angle da = snapped->object->get_rotation()-snapped->rot;
+			Transform trans = Transform::rotation(da, Vector(0, 0, 1));
 			const Vector &sp = snapped->object->get_position();
 			for(vector<MObject>::iterator i=objects.begin(); i!=objects.end(); ++i)
 			{
 				if(&*i==snapped)
 					continue;
 
-				Vector dp(i->pos.x-snapped->pos.x, i->pos.y-snapped->pos.y, 0);
-				i->object->set_position(Vector(sp.x+c*dp.x-s*dp.y, sp.y+s*dp.x+c*dp.y, sp.z+i->pos.z-snapped->pos.z));
+				i->object->set_position(sp+trans.transform(i->pos-snapped->pos));
 				i->object->set_rotation(i->rot+da);
 			}
 		}
 	}
 	else if(mode==ROTATE)
 	{
-		float a = atan2(gpointer.y-center.y, gpointer.x-center.x);
+		Angle a = Geometry::atan2(gpointer.y-center.y, gpointer.x-center.x);
 		angle += a-rot_origin;
 		rot_origin = a;
 
+		Transform trans = Transform::rotation(angle, Vector(0, 0, 1));
 		for(vector<MObject>::iterator i=objects.begin(); i!=objects.end(); ++i)
 		{
-			float c = cos(angle);
-			float s = sin(angle);
-			i->object->set_position(Vector(center.x+c*i->pos.x-s*i->pos.y, center.y+s*i->pos.x+c*i->pos.y, center.z+i->pos.z));
+			i->object->set_position(center+trans.transform(i->pos));
 			i->object->set_rotation(angle+i->rot);
 		}
 	}
@@ -430,12 +418,12 @@ void Manipulator::axis_motion(unsigned axis, float value, float)
 		signal_status.emit(format("Elevation: %+.0fmm (%.0fmm)", dz*1000, (center.z+dz)*1000));
 
 		for(vector<MObject>::iterator i=objects.begin(); i!=objects.end(); ++i)
-			i->object->set_position(Vector(center.x+i->pos.x, center.y+i->pos.y, center.z+i->pos.z+dz));
+			i->object->set_position(center+i->pos+Vector(0, 0, dz));
 	}
 	else if(mode==EXTEND)
 	{
 		Vector pos;
-		float dir = 0;
+		Angle dir;
 		float length = 0;
 		for(vector<MObject>::iterator i=objects.begin(); i!=objects.end(); ++i)
 		{
@@ -446,18 +434,14 @@ void Manipulator::axis_motion(unsigned axis, float value, float)
 					continue;
 
 				Snap sn = i->object->get_snap_node(j);
-				float c = cos(sn.rotation);
-				float s = sin(sn.rotation);
-				float dx = gpointer.x-sn.position.x;
-				float dy = gpointer.y-sn.position.y;
+				Vector delta = rotated_vector(gpointer-sn.position, -sn.rotation);
 
-				float len = dx*c+dy*s;
-				if(len<length)
+				if(delta.x<length)
 					continue;
 
 				pos = sn.position;
 				dir = sn.rotation;
-				length = len;
+				length = delta.x;
 			}
 		}
 
@@ -553,11 +537,11 @@ void Manipulator::update_objects()
 		}
 	}
 
-	center = Vector((minp.x+maxp.x)/2, (minp.y+maxp.y)/2, minp.z);
+	center = (minp+maxp)/2.0f;
+	center.z = minp.z;
 	for(vector<MObject>::iterator i=objects.begin(); i!=objects.end(); ++i)
 	{
-		const Vector &p = i->object->get_position();
-		i->pos = Vector(p.x-center.x, p.y-center.y, p.z-center.z);
+		i->pos = i->object->get_position()-center;
 		i->rot = i->object->get_rotation();
 	}
 }
@@ -601,7 +585,7 @@ void Manipulator::set_slope(TrackOrder &track, float z, float dz)
 	}
 }
 
-vector<Track *> Manipulator::create_straight(const R2C2::Vector &start, float dir, float length, float limit)
+vector<Track *> Manipulator::create_straight(const R2C2::Vector &start, const Angle &dir, float length, float limit)
 {
 	const Catalogue::TrackMap &track_types = designer.get_catalogue().get_tracks();
 	std::map<float, const TrackType *> types_by_length;
@@ -662,8 +646,7 @@ vector<Track *> Manipulator::create_straight(const R2C2::Vector &start, float di
 	if(!lengths.empty())
 	{
 		Vector pos = start;
-		float c = cos(dir);
-		float s = sin(dir);
+		Transform trans = Transform::rotation(dir, Vector(0, 0, 1));
 		for(vector<float>::iterator i=lengths.begin(); i!=lengths.end(); ++i)
 		{
 			Track *track = new Track(designer.get_layout(), *get_item(types_by_length, *i));
@@ -674,8 +657,7 @@ vector<Track *> Manipulator::create_straight(const R2C2::Vector &start, float di
 				track->link_to(*trks.back());
 			trks.push_back(track);
 
-			pos.x += c**i;
-			pos.y += s**i;
+			pos += trans.transform(Vector(*i, 0, 0));
 		}
 	}
 
diff --git a/source/designer/manipulator.h b/source/designer/manipulator.h
index 12e551b..1c46af6 100644
--- a/source/designer/manipulator.h
+++ b/source/designer/manipulator.h
@@ -2,6 +2,7 @@
 #define MANIPULATOR_H_
 
 #include <sigc++/sigc++.h>
+#include "libr2c2/geometry.h"
 
 class Designer;
 class Selection;
@@ -22,7 +23,7 @@ private:
 	{
 		R2C2::Object *object;
 		R2C2::Vector pos;
-		float rot;
+		R2C2::Angle rot;
 
 		MObject(R2C2::Object *);
 	};
@@ -50,8 +51,8 @@ private:
 	R2C2::Vector gpointer;
 	Mode mode;
 	R2C2::Vector move_origin;
-	float angle;
-	float rot_origin;
+	R2C2::Angle angle;
+	R2C2::Angle rot_origin;
 	float elev_origin;
 	std::set<R2C2::Object *> neighbors;
 	std::vector<R2C2::Track *> extend_tracks;
@@ -75,7 +76,7 @@ private:
 	void update_objects();
 	void update_neighbors();
 	void set_slope(TrackOrder &, float, float);
-	std::vector<R2C2::Track *> create_straight(const R2C2::Vector &, float, float, float);
+	std::vector<R2C2::Track *> create_straight(const R2C2::Vector &, const R2C2::Angle &, float, float);
 };
 
 #endif
diff --git a/source/designer/measure.cpp b/source/designer/measure.cpp
index 69ada81..c7fead1 100644
--- a/source/designer/measure.cpp
+++ b/source/designer/measure.cpp
@@ -29,7 +29,7 @@ void Measure::button_press(float gx, float gy, unsigned btn)
 	if(btn==1)
 	{
 		ssnap.position = Vector(gx, gy, 0);
-		ssnap.rotation = 0;
+		ssnap.rotation = Angle::zero();
 		snap_to_tracks(ssnap);
 
 		state = ACTIVE;
@@ -61,18 +61,12 @@ void Measure::pointer_motion(float gx, float gy)
 
 	if(state!=STARTING)
 	{
-		Vector delta(pointer.x-ssnap.position.x, pointer.y-ssnap.position.y, 0);
-		float c = cos(ssnap.rotation);
-		float s = sin(ssnap.rotation);
+		Vector delta = rotated_vector(pointer-ssnap.position, -ssnap.rotation);
 
-		par_dist = delta.x*c+delta.y*s;
-		perp_dist = delta.x*s-delta.y*c;
+		par_dist = delta.x;
+		perp_dist = delta.y;
 
-		adiff = sn.rotation-ssnap.rotation+M_PI;
-		while(adiff<-M_PI)
-			adiff += M_PI*2;
-		while(adiff>M_PI)
-			adiff -= M_PI*2;
+		adiff = wrap_balanced(sn.rotation-ssnap.rotation+Angle::half_turn());
 
 		update_mesh();
 
@@ -87,7 +81,7 @@ void Measure::render(GL::Renderer &renderer, const GL::Tag &) const
 
 	GL::Renderer::Push push(renderer);
 	const Vector &pos = (state==ACTIVE ? ssnap.position : pointer);
-	renderer.matrix_stack() *= GL::Matrix::translation(pos.x, pos.y, pos.z);
+	renderer.matrix_stack() *= GL::Matrix::translation(pos);
 
 	mesh.draw(renderer);
 }
diff --git a/source/designer/measure.h b/source/designer/measure.h
index 022c314..c569698 100644
--- a/source/designer/measure.h
+++ b/source/designer/measure.h
@@ -3,6 +3,7 @@
 
 #include <sigc++/sigc++.h>
 #include <msp/gl/renderable.h>
+#include "libr2c2/geometry.h"
 
 class Designer;
 
@@ -26,7 +27,7 @@ private:
 	R2C2::Snap ssnap;
 	float par_dist;
 	float perp_dist;
-	float adiff;
+	R2C2::Angle adiff;
 	State state;
 	Msp::GL::Mesh mesh;
 
@@ -34,7 +35,7 @@ public:
 	Measure(Designer &);
 	float get_parallel_distance() const { return par_dist; }
 	float get_perpendicular_distance() const { return perp_dist; }
-	float get_angle_difference() const { return adiff; }
+	const R2C2::Angle &get_angle_difference() const { return adiff; }
 	void start();
 	void button_press(float, float, unsigned);
 	void pointer_motion(float, float);
diff --git a/source/designer/svgexporter.cpp b/source/designer/svgexporter.cpp
index 62e869c..ca95f90 100644
--- a/source/designer/svgexporter.cpp
+++ b/source/designer/svgexporter.cpp
@@ -67,8 +67,8 @@ void SvgExporter::save(const string &fn)
 void SvgExporter::save_track(const Track &track, xmlpp::Element &group)
 {
 	const Vector &pos = track.get_position();
-	float rot = track.get_rotation();
-	string transform = format("translate(%.3f %.3f) rotate(%.3f)", pos.x*1000, -pos.y*1000, -rot*180/M_PI);
+	const Angle &rot = track.get_rotation();
+	string transform = format("translate(%.3f %.3f) rotate(%.3f)", pos.x*1000, -pos.y*1000, -rot.degrees());
 	group.set_attribute("transform", transform);
 
 	const TrackType &type = track.get_type();
@@ -79,12 +79,11 @@ void SvgExporter::save_track(const Track &track, xmlpp::Element &group)
 		xmlpp::Element *elem = group.add_child("path");
 		elem->set_attribute("class", "endpoint");
 
-		float dx = -sin(i->dir)*gauge;
-		float dy = -cos(i->dir)*gauge;
+		Vector delta = rotated_vector(Vector(0, gauge, 0), i->dir);
 
 		string data = format("M %.3f %.3f L %.3f %.3f",
-			i->pos.x*1000+dx, -i->pos.y*1000+dy,
-			i->pos.x*1000-dx, -i->pos.y*1000-dy);
+			i->pos.x*1000+delta.x, -i->pos.y*1000-delta.y,
+			i->pos.x*1000-delta.x, -i->pos.y*1000+delta.y);
 		elem->set_attribute("d", data);
 	}
 
@@ -98,27 +97,23 @@ void SvgExporter::save_track(const Track &track, xmlpp::Element &group)
 			xmlpp::Element *elem = group.add_child("path");
 			elem->set_attribute("class", "rail");
 
-			float cs = cos(start.dir);
-			float ss = sin(start.dir);
-			float ce = cos(end.dir);
-			float se = sin(end.dir);
-			float dx1 = -ss*(gauge+rail_width)*0.5;
-			float dy1 = -cs*(gauge+rail_width)*0.5;
-			float dx2 = -se*(gauge+rail_width)*0.5;
-			float dy2 = -ce*(gauge+rail_width)*0.5;
+			Vector delta1 = rotated_vector(Vector(0, (gauge+rail_width)*0.5, 0), start.dir);
+			Vector delta2 = rotated_vector(Vector(0, (gauge+rail_width)*0.5, 0), end.dir);
 			// Largely an educated guess, but seems to be accurate enough
 			float clen = i->get_length()*1000/2.9;
+			Vector ctrl1 = rotated_vector(Vector(clen, 0, 0), start.dir);
+			Vector ctrl2 = rotated_vector(Vector(clen, 0, 0), end.dir);
 
 			string data = format("M %.3f %.3f C %.3f %.3f %.3f %.3f %.3f %.3f")
-				(start.pos.x*1000+dx1)(-start.pos.y*1000+dy1)
-				(start.pos.x*1000+dx1+cs*clen)(-start.pos.y*1000+dy1-ss*clen)
-				(end.pos.x*1000+dx2-ce*clen)(-end.pos.y*1000+dy2+se*clen)
-				(end.pos.x*1000+dx2)(-end.pos.y*1000+dy2).str();
+				(start.pos.x*1000+delta1.x)(-start.pos.y*1000-delta1.y)
+				(start.pos.x*1000+delta1.x+ctrl1.x)(-start.pos.y*1000-delta1.y-ctrl1.y)
+				(end.pos.x*1000+delta2.x-ctrl2.x)(-end.pos.y*1000-delta2.y+ctrl2.y)
+				(end.pos.x*1000+delta2.x)(-end.pos.y*1000-delta2.y).str();
 			data += format(" M %.3f %.3f C %.3f %.3f %.3f %.3f %.3f %.3f")
-				(start.pos.x*1000-dx1)(-start.pos.y*1000-dy1)
-				(start.pos.x*1000-dx1+cs*clen)(-start.pos.y*1000-dy1-ss*clen)
-				(end.pos.x*1000-dx2-ce*clen)(-end.pos.y*1000-dy2+se*clen)
-				(end.pos.x*1000-dx2)(-end.pos.y*1000-dy2).str();
+				(start.pos.x*1000-delta1.x)(-start.pos.y*1000+delta1.y)
+				(start.pos.x*1000-delta1.x+ctrl1.x)(-start.pos.y*1000+delta1.y-ctrl1.y)
+				(end.pos.x*1000-delta2.x-ctrl2.x)(-end.pos.y*1000+delta2.y+ctrl2.y)
+				(end.pos.x*1000-delta2.x)(-end.pos.y*1000+delta2.y).str();
 			elem->set_attribute("d", data);
 		}
 		else
@@ -126,32 +121,30 @@ void SvgExporter::save_track(const Track &track, xmlpp::Element &group)
 			xmlpp::Element *elem = group.add_child("path");
 			elem->set_attribute("class", "rail");
 
-			float dx = -sin(start.dir)*(gauge+rail_width)*0.5;
-			float dy = -cos(start.dir)*(gauge+rail_width)*0.5;
+			Vector delta = rotated_vector(Vector(0, (gauge+rail_width)*0.5, 0), start.dir);
 
 			string data = format("M %.3f %.3f L %.3f %.3f",
-				start.pos.x*1000+dx, -start.pos.y*1000+dy,
-				end.pos.x*1000+dx, -end.pos.y*1000+dy);
+				start.pos.x*1000+delta.x, -start.pos.y*1000-delta.y,
+				end.pos.x*1000+delta.x, -end.pos.y*1000-delta.y);
 			data += format(" M %.3f %.3f L %.3f %.3f",
-				start.pos.x*1000-dx, -start.pos.y*1000-dy,
-				end.pos.x*1000-dx, -end.pos.y*1000-dy);
+				start.pos.x*1000-delta.x, -start.pos.y*1000+delta.y,
+				end.pos.x*1000-delta.x, -end.pos.y*1000+delta.y);
 			elem->set_attribute("d", data);
 		}
 	}
 
 	TrackPoint label_pt = parts.front().get_point(parts.front().get_length()/2);
 
-	while(rot+label_pt.dir>M_PI/2)
-		label_pt.dir -= M_PI;
-	while(rot+label_pt.dir<-M_PI/2)
-		label_pt.dir += M_PI;
+	label_pt.dir.wrap_with_base(-Angle::quarter_turn());
+	if(label_pt.dir>Angle::quarter_turn())
+		label_pt.dir -= Angle::half_turn();
 
-	label_pt.pos.x = label_pt.pos.x*1000+sin(label_pt.dir)*gauge*0.25;
-	label_pt.pos.y = label_pt.pos.y*1000-cos(label_pt.dir)*gauge*0.25;
+	label_pt.pos *= 1000;
+	label_pt.pos += rotated_vector(Vector(0, -gauge*0.25, 0), label_pt.dir);
 
 	xmlpp::Element *elem = group.add_child("text");
 	elem->set_attribute("class", "artnr");
 	elem->set_attribute("transform", format("translate(%.3f %.3f) rotate(%.3f)",
-		label_pt.pos.x, -label_pt.pos.y, -label_pt.dir*180/M_PI));
+		label_pt.pos.x, -label_pt.pos.y, -label_pt.dir.degrees()));
 	elem->set_child_text(track.get_type().get_article_number().str());
 }
diff --git a/source/designer/trackwrap.cpp b/source/designer/trackwrap.cpp
index 863434f..398285f 100644
--- a/source/designer/trackwrap.cpp
+++ b/source/designer/trackwrap.cpp
@@ -28,7 +28,7 @@ void TrackWrap::render(GL::Renderer &renderer, const GL::Tag &) const
 	{
 		GL::MatrixStack::Push push(renderer.matrix_stack());
 		const Vector &pos = i->track->get_position();
-		renderer.matrix_stack() *= GL::Matrix::translation(pos.x, pos.y, pos.z);
+		renderer.matrix_stack() *= GL::Matrix::translation(pos);
 		renderer.matrix_stack() *= GL::Matrix::rotation(i->track->get_rotation(), 0, 0, 1);
 		i->mesh->draw(renderer);
 	}
@@ -56,22 +56,19 @@ GL::Mesh &TrackWrap::get_mesh(const TrackType &type)
 	const TrackType3D &type3d = layout.get_catalogue().get_track(type);
 
 	float min_area = -1;
-	float angle = 0;
+	Angle angle;
 	Vector center;
 	float width = 0;
 	float height = 0;
-	for(float a=0; a<M_PI; a+=0.01)
+	for(Angle a; a<Angle::half_turn(); a+=Angle::from_radians(0.01))
 	{
 		Vector minp, maxp;
 		type3d.get_bounds(a, minp, maxp);
 		float area = (maxp.x-minp.x)*(maxp.y-minp.y);
 		if(area<min_area || min_area<0)
 		{
-			float c = cos(a);
-			float s = sin(a);
-			float x = (minp.x+maxp.x)/2;
-			float y = (minp.y+maxp.y)/2;
-			center = Vector(c*x-s*y, s*x+c*y, minp.z);
+			center = rotated_vector((minp+maxp)/2.0f, a);
+			center.z = minp.z;
 			angle = a;
 			width = maxp.x-minp.x+0.01;
 			height = maxp.y-minp.y+0.01;
diff --git a/source/engineer/engineer.cpp b/source/engineer/engineer.cpp
index b0296a1..ad26e50 100644
--- a/source/engineer/engineer.cpp
+++ b/source/engineer/engineer.cpp
@@ -232,8 +232,8 @@ void Engineer::tick()
 
 		Snap sn = picking_track->get_snap_node(picking_entry);
 
-		GL::MatrixStack::modelview() *= GL::Matrix::translation(sn.position.x, sn.position.y, sn.position.z+0.03);
-		GL::MatrixStack::modelview() *= GL::Matrix::rotation(sn.rotation+M_PI, 0, 0, 1);
+		GL::MatrixStack::modelview() *= GL::Matrix::translation(sn.position+GL::Vector3(0, 0, 0.03));
+		GL::MatrixStack::modelview() *= GL::Matrix::rotation(sn.rotation+Angle::half_turn(), 0, 0, 1);
 
 		arrow_mesh.draw();
 	}
@@ -313,7 +313,7 @@ void Engineer::view_all()
 	float best_score = 0;
 	GL::Vector3 pos;
 	GL::Vector3 up;
-	for(float angle=0; angle<M_PI; angle+=0.01)
+	for(Angle angle; angle<Angle::half_turn(); angle+=Angle::from_radians(0.01))
 	{
 		float min_x = 0;
 		float max_x = 0;
diff --git a/source/engineer/trainview.cpp b/source/engineer/trainview.cpp
index 79f9c94..dffa04d 100644
--- a/source/engineer/trainview.cpp
+++ b/source/engineer/trainview.cpp
@@ -86,26 +86,26 @@ void TrainView::prepare()
 {
 	const Vehicle &veh = train.get_vehicle(0);
 	const Vector &pos = veh.get_position();
-	float angle = veh.get_rotation();
+	Angle angle = veh.get_rotation();
 	if(!forward)
-		angle += M_PI;
-	float c = cos(angle);
-	float s = sin(angle);
+		angle += Angle::half_turn();
+	Vector fwd_vec = rotated_vector(Vector(1, 0, 0), angle);
+	Vector side_vec = rotated_vector(Vector(0, -1, 0), angle);
 	float l = veh.get_type().get_length();
 
 	switch(mode)
 	{
 	case ROOF:
-		camera.set_position(GL::Vector3(pos.x-l*c, pos.y-l*s, pos.z+0.07));
-		camera.set_look_direction(GL::Vector3(c, s, -0.2));
+		camera.set_position(pos-l*fwd_vec+Vector(0, 0, 0.07));
+		camera.set_look_direction(fwd_vec-Vector(0, 0, -0.2));
 		break;
 	case SIDE:
-		camera.set_position(GL::Vector3(pos.x-l*0.8*c+0.05*s, pos.y-l*0.8*s-0.05*c, pos.z+0.03));
-		camera.set_look_direction(GL::Vector3(c-0.2*s, s+0.2*c, 0));
+		camera.set_position(pos-0.8f*fwd_vec+0.05f*side_vec+Vector(0, 0, 0.03));
+		camera.set_look_direction(fwd_vec-side_vec*0.2f);
 		break;
 	case HEAD:
-		camera.set_position(GL::Vector3(pos.x+l*0.55*c, pos.y+l*0.55*s, pos.z+0.03));
-		camera.set_look_direction(GL::Vector3(c, s, 0));
+		camera.set_position(pos+fwd_vec*(l*0.55f)+Vector(0, 0, 0.03));
+		camera.set_look_direction(fwd_vec);
 		break;
 	}
 
diff --git a/source/libr2c2/catalogue.cpp b/source/libr2c2/catalogue.cpp
index 3fb314a..9310c4c 100644
--- a/source/libr2c2/catalogue.cpp
+++ b/source/libr2c2/catalogue.cpp
@@ -89,8 +89,8 @@ void Catalogue::Loader::gauge(float g)
 {
 	obj.gauge = g/1000;
 	obj.path_profile = Profile();
-	obj.path_profile.append_vertex(Vector(0.1*obj.gauge, 0), false);
-	obj.path_profile.append_vertex(Vector(-0.1*obj.gauge, 0), false);
+	obj.path_profile.append_vertex(Vector(0.1*obj.gauge, 0, 0), false);
+	obj.path_profile.append_vertex(Vector(-0.1*obj.gauge, 0, 0), false);
 }
 
 void Catalogue::Loader::layout()
diff --git a/source/libr2c2/centralstation.cpp b/source/libr2c2/centralstation.cpp
index ee5aaee..ac553ab 100644
--- a/source/libr2c2/centralstation.cpp
+++ b/source/libr2c2/centralstation.cpp
@@ -152,12 +152,12 @@ void CentralStation::add_turnout(unsigned addr, const TrackType &type)
 	{
 		TrackPoint start = i->get_point(0);
 		TrackPoint end = i->get_point(i->get_length());
-		if(end.dir>start.dir+0.01 || end.dir<start.dir-0.01)
+		if(abs(end.dir-start.dir).radians()<0.01)
 		{
 			(end.dir>start.dir ? left : right) = true;
 			straight &= ~(1<<i->get_path());
 		}
-		else if(start.dir<-0.01 || start.dir>0.01)
+		else if(abs(start.dir).radians()>0.01)
 			cross = true;
 	}
 
diff --git a/source/libr2c2/geometry.h b/source/libr2c2/geometry.h
index 67c5412..09044e3 100644
--- a/source/libr2c2/geometry.h
+++ b/source/libr2c2/geometry.h
@@ -3,28 +3,34 @@
 
 #include <cmath>
 #include <vector>
+#include <msp/geometry/affinetransformation.h>
+#include <msp/geometry/angle.h>
+#include <msp/linal/vector.h>
 
 namespace R2C2 {
 
-struct Vector
-{
-	float x, y, z;
-
-	Vector(): x(0), y(0), z(0) { }
-	Vector(float x_, float y_): x(x_), y(y_), z(0) { }
-	Vector(float x_, float y_, float z_): x(x_), y(y_), z(z_) { }
-};
+typedef Msp::LinAl::Vector<float, 3> Vector;
 
 inline float distance(const Vector &p, const Vector &q)
-{ return sqrt((p.x-q.x)*(p.x-q.x) + (p.y-q.y)*(p.y-q.y) + (p.z-q.z)*(p.z-q.z)); }
+{ return (p-q).norm(); }
+
+typedef Msp::Geometry::Angle<float> Angle;
+
+typedef Msp::Geometry::AffineTransformation<float, 3> Transform;
+
+inline Vector rotated_vector(const Vector &v, const Angle &a)
+{ return Transform::rotation(a, Vector(0, 0, 1)).transform(v); }
+
+inline Vector vector_at_angle(const Angle &a)
+{ return rotated_vector(Vector(1, 0, 0), a); }
 
 struct TrackPoint
 {
 	Vector pos;
-	float dir;
+	Angle dir;
 	float grade;
 
-	TrackPoint(): dir(0), grade(0) { }
+	TrackPoint(): grade(0) { }
 };
 
 } // namespace R2C2
diff --git a/source/libr2c2/layout.h b/source/libr2c2/layout.h
index 5573673..48a2796 100644
--- a/source/libr2c2/layout.h
+++ b/source/libr2c2/layout.h
@@ -6,6 +6,7 @@
 #include <msp/datafile/objectloader.h>
 #include <msp/time/timestamp.h>
 #include "block.h"
+#include "geometry.h"
 
 namespace R2C2 {
 
@@ -16,7 +17,6 @@ class Route;
 class Signal;
 class Track;
 class Train;
-class Vector;
 class Vehicle;
 class Zone;
 
diff --git a/source/libr2c2/object.cpp b/source/libr2c2/object.cpp
index ba1810f..cc56ede 100644
--- a/source/libr2c2/object.cpp
+++ b/source/libr2c2/object.cpp
@@ -18,8 +18,8 @@ bool Object::snap(Snap &sn, float limit, SnapType what) const
 		for(unsigned i=0; i<nsn; ++i)
 		{
 			Snap node = get_snap_node(i);
-			Vector d(sn.position.x-node.position.x, sn.position.y-node.position.y, sn.position.z-node.position.z);
-			if(d.x*d.x+d.y*d.y<limit*limit)
+			Vector span = sn.position-node.position;
+			if(dot(span, span)<limit*limit)
 			{
 				sn = node;
 				return true;
@@ -45,11 +45,9 @@ bool Object::snap_to(const Object &other, float limit, SnapType what)
 		Snap ssn = sn;
 		if(other.snap(ssn, limit, what))
 		{
-			set_rotation(rotation+ssn.rotation-sn.rotation-M_PI);
+			set_rotation(rotation+ssn.rotation-sn.rotation-Angle::half_turn());
 			sn = get_snap_node(i);
-			position.x += ssn.position.x-sn.position.x;
-			position.y += ssn.position.y-sn.position.y;
-			position.z += ssn.position.z-sn.position.z;
+			position += ssn.position-sn.position;
 			return true;
 		}
 	}
diff --git a/source/libr2c2/object.h b/source/libr2c2/object.h
index 33b99a1..d36104f 100644
--- a/source/libr2c2/object.h
+++ b/source/libr2c2/object.h
@@ -14,7 +14,7 @@ class Object
 protected:
 	Layout &layout;
 	Vector position;
-	float rotation;
+	Angle rotation;
 
 	Object(Layout &l): layout(l) { }
 public:
@@ -24,9 +24,9 @@ public:
 	virtual const ObjectType &get_type() const = 0;
 	Layout &get_layout() const { return layout; }
 	virtual void set_position(const Vector &) = 0;
-	virtual void set_rotation(float) = 0;
+	virtual void set_rotation(const Angle &) = 0;
 	const Vector &get_position() const { return position; }
-	float get_rotation() const { return rotation; }
+	const Angle &get_rotation() const { return rotation; }
 	virtual Object *get_parent() const { return 0; }
 
 	virtual unsigned get_n_snap_nodes() const { return 0; }
diff --git a/source/libr2c2/profile.cpp b/source/libr2c2/profile.cpp
index a9951b2..907da8a 100644
--- a/source/libr2c2/profile.cpp
+++ b/source/libr2c2/profile.cpp
@@ -17,20 +17,16 @@ void Profile::append_vertex(const Vector &p, bool smooth)
 
 	if(!vertices.empty())
 	{
-		float dx = p.x-vertices.back().pos.x;
-		float dy = p.y-vertices.back().pos.y;
-		float len = sqrt(dx*dx+dy*dy);
-		v.normal.x = dy/len;
-		v.normal.y = -dx/len;
+		Vector span = p-vertices.back().pos;
+		span.normalize();
+		v.normal.x = span.y;
+		v.normal.y = -span.x;
 
 		if(vertices.back().smooth)
 		{
 			Vector &n = vertices.back().normal;
-			n.x += v.normal.x;
-			n.y += v.normal.y;
-			len = sqrt(n.x*n.x+n.y*n.y);
-			n.x /= len;
-			n.y /= len;
+			n += v.normal;
+			n.normalize();
 		}
 		else
 			vertices.back().normal = v.normal;
@@ -69,12 +65,12 @@ Profile::Loader::Loader(Profile &p):
 
 void Profile::Loader::point(float x, float y)
 {
-	obj.append_vertex(Vector(x/1000, y/1000), false);
+	obj.append_vertex(Vector(x/1000, y/1000, 0), false);
 }
 
 void Profile::Loader::smooth_point(float x, float y)
 {
-	obj.append_vertex(Vector(x/1000, y/1000), true);
+	obj.append_vertex(Vector(x/1000, y/1000, 0), true);
 }
 
 } // namespace R2C2
diff --git a/source/libr2c2/signal.cpp b/source/libr2c2/signal.cpp
index 650c362..3494467 100644
--- a/source/libr2c2/signal.cpp
+++ b/source/libr2c2/signal.cpp
@@ -80,12 +80,8 @@ void Signal::normalize_location()
 	unsigned n_endpoints = track->get_type().get_endpoints().size();
 	for(unsigned j=0; j<n_endpoints; ++j)
 	{
-		float a = track->get_snap_node(j).rotation-rotation;
-		while(a<-M_PI/2)
-			a += M_PI*2;
-		while(a>M_PI*3/2)
-			a -= M_PI*2;
-		if(a>=M_PI/2)
+		Angle a = wrap_with_base(track->get_snap_node(j).rotation-rotation, -Angle::quarter_turn());
+		if(a>=Angle::quarter_turn())
 		{
 			BlockIter biter = TrackIter(track, j).block_iter();
 			entry = biter.entry();
@@ -93,19 +89,11 @@ void Signal::normalize_location()
 	}
 }
 
-void Signal::set_rotation(float r)
+void Signal::set_rotation(const Angle &r)
 {
-	float a = rotation-r;
-	while(a>M_PI*3/2)
-		a -= M_PI*2;
-	while(a<-M_PI/2)
-		a += M_PI*2;
-	if(a>=M_PI/2)
-	{
-		rotation += M_PI;
-		if(rotation>M_PI*2)
-			rotation -= M_PI*2;
-	}
+	Angle a = wrap_with_base(rotation-r, -Angle::quarter_turn());
+	if(a>=Angle::quarter_turn())
+		rotation = wrap_positive(rotation+Angle::half_turn());
 
 	normalize_location();
 }
@@ -137,15 +125,14 @@ SnapType Signal::get_default_snap_type_to(const Object &other) const
 bool Signal::collide_ray(const Vector &start, const Vector &ray) const
 {
 	// XXX Totally hardcoded stuff, should be replaced with a geometry system
-	Vector center = position;
-	center.x += sin(rotation)*0.035;
-	center.y -= cos(rotation)*0.035;
-	Vector d(center.x-start.x, center.y-start.y);
-	float x = (d.x*ray.x+d.y*ray.y)/(ray.x*ray.x+ray.y*ray.y);
-	Vector nearest(start.x+ray.x*x-center.x, start.y+ray.y*x-center.y, start.z+ray.z*x-center.z);
-	if(nearest.z<0|| nearest.z>0.12)
+	Vector center = position+rotated_vector(Vector(0, -0.035, 0), rotation);
+	Vector span = center-start;
+	float x = (span.x*ray.x+span.y*ray.y)/(ray.x*ray.x+ray.y*ray.y);
+	Vector nearest = start+ray*x-center;
+	if(nearest.z<0 || nearest.z>0.12)
 		return false;
-	return nearest.x*nearest.x+nearest.y*nearest.y<0.0001;
+	nearest.z = 0;
+	return dot(nearest, nearest)<0.0001;
 }
 
 void Signal::tick(const Time::TimeDelta &)
@@ -225,7 +212,7 @@ void Signal::reset()
 void Signal::save(list<DataFile::Statement> &st) const
 {
 	st.push_back((DataFile::Statement("position"), position.x, position.y, position.z));
-	st.push_back((DataFile::Statement("rotation"), rotation));
+	st.push_back((DataFile::Statement("rotation"), rotation.radians()));
 	if(address)
 		st.push_back((DataFile::Statement("address"), address));
 }
@@ -251,7 +238,7 @@ void Signal::Loader::position(float x, float y, float z)
 
 void Signal::Loader::rotation(float d)
 {
-	obj.set_rotation(d);
+	obj.set_rotation(Angle::from_radians(d));
 }
 
 } // namespace R2C2
diff --git a/source/libr2c2/signal.h b/source/libr2c2/signal.h
index b604951..8adbd1b 100644
--- a/source/libr2c2/signal.h
+++ b/source/libr2c2/signal.h
@@ -48,7 +48,7 @@ public:
 
 	void set_address(unsigned);
 	virtual void set_position(const Vector &);
-	virtual void set_rotation(float);
+	virtual void set_rotation(const Angle &);
 private:
 	void normalize_location();
 public:
diff --git a/source/libr2c2/snap.h b/source/libr2c2/snap.h
index 844af3d..25777b3 100644
--- a/source/libr2c2/snap.h
+++ b/source/libr2c2/snap.h
@@ -1,6 +1,8 @@
 #ifndef LIBR2C2_SNAP_H_
 #define LIBR2C2_SNAP_H_
 
+#include "geometry.h"
+
 namespace R2C2 {
 
 enum SnapType
@@ -14,7 +16,7 @@ enum SnapType
 struct Snap
 {
 	Vector position;
-	float rotation;
+	Angle rotation;
 };
 
 inline SnapType operator|(SnapType s1, SnapType s2)
diff --git a/source/libr2c2/track.cpp b/source/libr2c2/track.cpp
index 6a6cbd9..7b5e1a2 100644
--- a/source/libr2c2/track.cpp
+++ b/source/libr2c2/track.cpp
@@ -74,13 +74,9 @@ void Track::set_position(const Vector &p)
 			(*i)->check_slope();
 }
 
-void Track::set_rotation(float r)
+void Track::set_rotation(const Angle &r)
 {
-	rotation = r;
-	while(rotation<0)
-		rotation += M_PI*2;
-	while(rotation>M_PI*2)
-		rotation -= M_PI*2;
+	rotation = wrap_positive(r);
 }
 
 void Track::set_slope(float s)
@@ -161,10 +157,8 @@ void Track::set_active_path(unsigned p)
 TrackPoint Track::get_point(unsigned epi, unsigned path, float d) const
 {
 	TrackPoint p = type.get_point(epi, path, d);
-	float c = cos(rotation);
-	float s = sin(rotation);
 
-	p.pos = Vector(position.x+c*p.pos.x-s*p.pos.y, position.y+s*p.pos.x+c*p.pos.y, position.z);
+	p.pos = position+rotated_vector(p.pos, rotation);
 	p.dir += rotation;
 	if(type.get_endpoints().size()==2)
 	{
@@ -204,10 +198,7 @@ Snap Track::get_snap_node(unsigned i) const
 	Snap result;
 	const TrackType::Endpoint &ep = eps[i];
 
-	float c = cos(rotation);
-	float s = sin(rotation);
-
-	result.position = Vector(position.x+c*ep.pos.x-s*ep.pos.y, position.y+s*ep.pos.x+c*ep.pos.y, position.z);
+	result.position = position+rotated_vector(ep.pos, rotation);
 	if(eps.size()==2 && i==1)
 		result.position.z += slope;
 
@@ -223,16 +214,13 @@ bool Track::snap(Snap &sn, float limit, SnapType what) const
 
 	if(what&SNAP_SEGMENT)
 	{
-		Vector local(sn.position.x-position.x, sn.position.y-position.y, sn.position.z-position.z);
-		float c = cos(rotation);
-		float s = sin(rotation);
-		local = Vector(c*local.x+s*local.y, c*local.y-s*local.x, local.z);
+		Vector local = rotated_vector(sn.position-position, -rotation);
 
 		TrackPoint tp = type.get_nearest_point(local);
-		Vector d(local.x-tp.pos.x, local.y-tp.pos.y, local.z-tp.pos.z);
-		if(d.x*d.x+d.y*d.y+d.z*d.z<=limit*limit)
+		Vector span = local-tp.pos;
+		if(dot(span, span)<=limit*limit)
 		{
-			sn.position = Vector(position.x+tp.pos.x*c-tp.pos.y*s, position.y+tp.pos.y*c+tp.pos.x*s, position.z+tp.pos.z);
+			sn.position = position+rotated_vector(tp.pos, rotation);
 			sn.rotation = tp.dir+rotation;
 			return true;
 		}
@@ -290,14 +278,10 @@ bool Track::link_to(Object &other)
 		for(unsigned j=0; j<other_nsn; ++j)
 		{
 			Snap osn = other.get_snap_node(j);
-			Vector d(osn.position.x-sn.position.x, osn.position.y-sn.position.y, osn.position.z-sn.position.z);
-			float da = osn.rotation-sn.rotation-M_PI;
-			while(da<-M_PI)
-				da += M_PI*2;
-			while(da>M_PI)
-				da -= M_PI*2;
-
-			if(d.x*d.x+d.y*d.y<limit && d.z*d.z<limit && da>-0.01 && da<0.01)
+			Vector span = osn.position-sn.position;
+			Angle da = wrap_balanced(osn.rotation-sn.rotation-Angle::half_turn());
+
+			if(dot(span, span)<limit && abs(da).radians()<0.01)
 			{
 				break_link(i);
 				links[i] = otrack;
@@ -335,11 +319,9 @@ bool Track::break_link(unsigned i)
 
 bool Track::collide_ray(const Vector &start, const Vector &ray) const
 {
-	Vector local_start(start.x-position.x, start.y-position.y, start.z-position.z);
-	float c = cos(rotation);
-	float s = sin(rotation);
-	local_start = Vector(c*local_start.x+s*local_start.y, c*local_start.y-s*local_start.x, local_start.z);
-	Vector local_ray(c*ray.x+s*ray.y, c*ray.y-s*ray.x, ray.z);
+	Transform trans = Transform::rotation(-rotation, Vector(0, 0, 1));
+	Vector local_start = trans.transform(start-position);
+	Vector local_ray = trans.transform(ray);
 
 	float width = layout.get_catalogue().get_ballast_profile().get_width();
 
@@ -349,7 +331,7 @@ bool Track::collide_ray(const Vector &start, const Vector &ray) const
 void Track::save(list<DataFile::Statement> &st) const
 {
 	st.push_back((DataFile::Statement("position"), position.x, position.y, position.z));
-	st.push_back((DataFile::Statement("rotation"), rotation));
+	st.push_back((DataFile::Statement("rotation"), rotation.radians()));
 	st.push_back((DataFile::Statement("slope"), slope));
 	if(turnout_id)
 		st.push_back((DataFile::Statement("turnout_id"), turnout_id));
@@ -391,7 +373,7 @@ void Track::Loader::position(float x, float y, float z)
 
 void Track::Loader::rotation(float r)
 {
-	obj.rotation = r;
+	obj.rotation = Angle::from_radians(r);
 }
 
 void Track::Loader::sensor_id(unsigned id)
diff --git a/source/libr2c2/track.h b/source/libr2c2/track.h
index 13409d5..7dfa752 100644
--- a/source/libr2c2/track.h
+++ b/source/libr2c2/track.h
@@ -54,7 +54,7 @@ public:
 	void set_block(Block *);
 	Block &get_block() const;
 	virtual void set_position(const Vector &);
-	virtual void set_rotation(float);
+	virtual void set_rotation(const Angle &);
 	void set_slope(float);
 	void set_flex(bool);
 	float get_slope() const { return slope; }
diff --git a/source/libr2c2/trackpart.cpp b/source/libr2c2/trackpart.cpp
index 71845bb..d8dac07 100644
--- a/source/libr2c2/trackpart.cpp
+++ b/source/libr2c2/trackpart.cpp
@@ -7,7 +7,6 @@ using namespace Msp;
 namespace R2C2 {
 
 TrackPart::TrackPart():
-	dir(0),
 	length(0),
 	radius(0),
 	path(0),
@@ -29,19 +28,17 @@ TrackPoint TrackPart::get_point(float d) const
 {
 	TrackPoint result;
 
+	Transform dir_trans = Transform::rotation(dir, Vector(0, 0, 1));
 	if(radius)
 	{
-		float a = d/radius;
-		float c = cos(a);
-		float s = sin(a);
-		float rx = radius*sin(dir);
-		float ry = -radius*cos(dir);
-		result.pos = Vector(pos.x+c*rx-s*ry-rx, pos.y+c*ry+s*rx-ry);
+		Angle a = Angle::from_radians(d/radius);
+		Vector r = dir_trans.transform(Vector(0, -radius, 0));
+		result.pos = pos-r+Transform::rotation(a, Vector(0, 0, 1)).transform(r);
 		result.dir = dir+a;
 	}
 	else
 	{
-		result.pos = Vector(pos.x+cos(dir)*d, pos.y+sin(dir)*d);
+		result.pos = pos+dir_trans.transform(Vector(d, 0, 0));
 		result.dir = dir;
 	}
 
@@ -51,29 +48,27 @@ TrackPoint TrackPart::get_point(float d) const
 TrackPoint TrackPart::get_nearest_point(const Vector &p) const
 {
 	TrackPoint tp;
-	float c = cos(dir);
-	float s = sin(dir);
+	Transform dir_trans = Transform::rotation(dir, Vector(0, 0, 1));
 	if(radius)
 	{
-		Vector v(p.x-pos.x+radius*s, p.y-pos.y-radius*c);
-		float a = atan2(v.y, v.x)+M_PI/2-dir;
+		Vector r = dir_trans.transform(Vector(0, -radius, 0));
+		Vector v = p-pos+r;
+		Angle a = Geometry::atan2(v.y, v.x)+Angle::quarter_turn()-dir;
 		if(radius<0)
-			a = M_PI-a;
-		while(a<length/2-M_PI)
-			a += M_PI*2;
-		while(a>length/2+M_PI)
-			a -= M_PI*2;
-		a = min(max(a, 0.0f), length);
+			a = Angle::half_turn()-a;
+		a.wrap_with_base(Angle::from_radians(length/2)-Angle::half_turn());
+		a = min(max(a, Angle::zero()), Angle::from_radians(length));
 		if(radius<0)	
 			a = -a;
-		tp.pos = Vector(pos.x+radius*(sin(dir+a)-s), pos.y+radius*(c-cos(dir+a)));
+		tp.pos = pos-r+Transform::rotation(a, Vector(0, 0, 1)).transform(r);
 		tp.dir = dir+a;
 	}
 	else
 	{
-		Vector v(p.x-pos.x, p.y-pos.y);
-		float d = min(max(c*v.x+s*v.y, 0.0f), length);
-		tp.pos = Vector(pos.x+c*d, pos.y+s*d);
+		Vector v = p-pos;
+		Vector dir_vec = dir_trans.transform(Vector(1, 0, 0));
+		float d = min(max(dot(dir_vec, v), 0.0f), length);
+		tp.pos = pos+dir_vec*d;
 		tp.dir = dir;
 	}
 	return tp;
@@ -90,16 +85,11 @@ void TrackPart::check_link(TrackPart &other)
 		{
 			TrackPoint p2 = other.get_point(j ? other.get_length() : 0);
 
-			float dx = p2.pos.x-p1.pos.x;
-			float dy = p2.pos.y-p1.pos.y;
+			Vector span = p2.pos-p1.pos;
 
-			float da = p2.dir-p1.dir+M_PI*((i+j+1)%2);
-			while(da>M_PI)
-				da -= M_PI*2;
-			while(da<-M_PI)
-				da += M_PI*2;
+			Angle da = wrap_balanced(p2.dir-p1.dir+Angle::half_turn()*float((i+j+1)%2));
 
-			if(dx*dx+dy*dy<1e-6 && da>=-0.01 && da<=0.01)
+			if(dot(span, span)<1e-6 && abs(da).radians()<=0.01)
 			{
 				links[i] = &other;
 				other.links[j] = this;
@@ -118,24 +108,22 @@ TrackPart *TrackPart::get_link(unsigned i) const
 
 bool TrackPart::collide_ray(const Vector &start, const Vector &ray, float width) const
 {
-	Vector local_start(start.x-pos.x, start.y-pos.y, start.z);
-	float c = cos(dir);
-	float s = sin(dir);
-	local_start = Vector(c*local_start.x+s*local_start.y, c*local_start.y-s*local_start.x, local_start.z);
-	Vector local_ray(c*ray.x+s*ray.y, c*ray.y-s*ray.x, ray.z);
+	Transform trans = Transform::rotation(dir, Vector(0, 0, -1));
+	Vector local_start = trans.transform(start);
+	Vector local_ray = trans.transform_linear(ray);
 
 	float d = -local_start.z/local_ray.z;
 	if(d<0)
 		return false;
 
-	Vector base(local_start.x+d*local_ray.x, local_start.y+d*local_ray.y);
+	Vector base = local_start+d*local_ray;
 
 	if(radius)
 	{
 		base.y -= radius;
 		if(radius<0)
 			base.y = -base.y;
-		float r = sqrt(base.x*base.x+base.y*base.y)-abs(radius);
+		float r = base.norm()-abs(radius);
 		float a = atan2(base.x, -base.y);
 		return (a>=0 && a<=length && r>=-width/2 && r<=width/2);
 	}
@@ -164,15 +152,13 @@ void TrackPart::Loader::finish()
 	else
 		obj.length /= 1000;
 
-	obj.pos.x /= 1000;
-	obj.pos.y /= 1000;
-	obj.dir *= M_PI/180;
+	obj.pos /= 1000;
 }
 
 void TrackPart::Loader::start(float x, float y, float d)
 {
-	obj.pos = Vector(x, y);
-	obj.dir = d;
+	obj.pos = Vector(x, y, 0);
+	obj.dir = Angle::from_degrees(d);
 }
 
 } // namespace R2C2
diff --git a/source/libr2c2/trackpart.h b/source/libr2c2/trackpart.h
index 787a6ca..9143ff8 100644
--- a/source/libr2c2/trackpart.h
+++ b/source/libr2c2/trackpart.h
@@ -20,7 +20,7 @@ public:
 
 private:
 	Vector pos;
-	float dir;
+	Angle dir;
 	float length;
 	float radius;
 	unsigned path;
diff --git a/source/libr2c2/tracktype.cpp b/source/libr2c2/tracktype.cpp
index 3be1ecb..fbed4c1 100644
--- a/source/libr2c2/tracktype.cpp
+++ b/source/libr2c2/tracktype.cpp
@@ -76,9 +76,8 @@ TrackPoint TrackType::get_point(unsigned epi, unsigned path, float d) const
 		for(unsigned j=0; j<n_part_eps; ++j)
 		{
 			TrackPoint p = i->get_point(j ? i->get_length() : 0);
-			float dx = p.pos.x-endpoints[epi].pos.x;
-			float dy = p.pos.y-endpoints[epi].pos.y;
-			if(dx*dx+dy*dy<1e-6)
+			Vector span = p.pos-endpoints[epi].pos;
+			if(dot(span, span)<1e-6)
 			{
 				part = &*i;
 				part_ep = j;
@@ -98,7 +97,7 @@ TrackPoint TrackType::get_point(unsigned epi, unsigned path, float d) const
 				d = plen-d;
 			TrackPoint p = part->get_point(d);
 			if(part_ep==1)
-				p.dir += M_PI;
+				p.dir += Angle::half_turn();
 			return p;
 		}
 		else
@@ -156,21 +155,16 @@ void TrackType::collect_endpoints()
 			{
 				TrackPoint p = i->get_point(j ? i->get_length() : 0);
 				if(j==0)
-					p.dir += M_PI;
+					p.dir += Angle::half_turn();
 
 				bool found = false;
 				for(vector<Endpoint>::iterator k=endpoints.begin(); k!=endpoints.end(); ++k)
 				{
-					float dx = k->pos.x-p.pos.x;
-					float dy = k->pos.y-p.pos.y;
+					Vector d = k->pos-p.pos;
 
-					float da = k->dir-p.dir;
-					while(da>M_PI)
-						da -= M_PI*2;
-					while(da<-M_PI)
-						da += M_PI*2;
+					Angle da = wrap_balanced(k->dir-p.dir);
 
-					if(dx*dx+dy*dy<1e-6 && da>-0.01 && da<0.01)
+					if(dot(d, d)<1e-6 && abs(da).radians()<0.01)
 					{
 						k->paths |= 1<<i->get_path();
 						found = true;
@@ -184,8 +178,8 @@ void TrackType::collect_endpoints()
 	}
 }
 
-TrackType::Endpoint::Endpoint(float x, float y, float d, unsigned p):
-	pos(x, y),
+TrackType::Endpoint::Endpoint(float x, float y, const Angle &d, unsigned p):
+	pos(x, y, 0),
 	dir(d),
 	paths(p)
 { }
diff --git a/source/libr2c2/tracktype.h b/source/libr2c2/tracktype.h
index afc8521..f446064 100644
--- a/source/libr2c2/tracktype.h
+++ b/source/libr2c2/tracktype.h
@@ -15,10 +15,10 @@ public:
 	struct Endpoint
 	{
 		Vector pos;
-		float dir;  // Direction outwards from the endpoint
+		Angle dir;  // Direction outwards from the endpoint
 		unsigned paths;
 
-		Endpoint(float, float, float, unsigned);
+		Endpoint(float, float, const Angle &, unsigned);
 
 		bool has_path(unsigned p) const { return paths&(1<<p); }
 		bool has_common_paths(const Endpoint &e) const { return paths&e.paths; }
diff --git a/source/libr2c2/vehicle.cpp b/source/libr2c2/vehicle.cpp
index d1819a0..ca2aa42 100644
--- a/source/libr2c2/vehicle.cpp
+++ b/source/libr2c2/vehicle.cpp
@@ -274,10 +274,10 @@ void Vehicle::check_sensor(float offset, unsigned &sensor)
 void Vehicle::turn_axles(float d)
 {
 	for(vector<Axle>::iterator i=axles.begin(); i!=axles.end(); ++i)
-		i->angle += d*2/i->type->wheel_dia;
+		i->angle += Angle::from_radians(d*2/i->type->wheel_dia);
 	for(vector<Bogie>::iterator i=bogies.begin(); i!=bogies.end(); ++i)
 		for(vector<Axle>::iterator j=i->axles.begin(); j!=i->axles.end(); ++j)
-			j->angle += d*2/j->type->wheel_dia;
+			j->angle += Angle::from_radians(d*2/j->type->wheel_dia);
 
 	update_rods();
 }
@@ -291,19 +291,17 @@ void Vehicle::update_rods()
 		else if(i->type->pivot==VehicleType::Rod::AXLE)
 		{
 			const Axle &axle = get_fixed_axle(i->type->pivot_index);
-			float c = cos(axle.angle);
-			float s = sin(axle.angle);
 			const Vector &pp = i->type->pivot_point;
-			i->position = Vector(axle.type->position+pp.x*c+pp.z*s, pp.y, axle.type->wheel_dia/2+pp.z*c-pp.x*s);
+			Transform trans = Transform::rotation(axle.angle, Vector(0, -1, 0));
+			i->position = Vector(axle.type->position, 0, axle.type->wheel_dia/2)+trans.transform(pp);
 		}
 		else if(i->type->pivot==VehicleType::Rod::ROD)
 		{
 			const Rod &prod = get_rod(i->type->pivot_index);
-			float c = cos(prod.angle);
-			float s = sin(prod.angle);
 			const Vector &pos = prod.position;
 			const Vector &off = i->type->pivot_point;
-			i->position = Vector(pos.x+off.x*c-off.z*s, pos.y+off.y, pos.z+off.z*c+off.x*s);
+			Transform trans = Transform::rotation(prod.angle, Vector(0, 1, 0));
+			i->position = pos+trans.transform(off);
 		}
 
 		if(i->type->connect_index>=0)
@@ -311,30 +309,27 @@ void Vehicle::update_rods()
 			Rod &crod = rods[i->type->connect_index];
 			if(i->type->limit==VehicleType::Rod::ROTATE && crod.type->limit==VehicleType::Rod::SLIDE_X)
 			{
-				float dx = (crod.position.x+i->type->connect_offset.x)-i->position.x;
-				float dz = (crod.position.z+i->type->connect_offset.z)-i->position.z;
-				float cd = sqrt(i->type->connect_point.x*i->type->connect_point.x+i->type->connect_point.z*i->type->connect_point.z);
-				float ca = atan2(i->type->connect_point.z, i->type->connect_point.x);
-				dx = sqrt(cd*cd-dz*dz)*(dx>0 ? 1 : -1);
-				i->angle = atan2(dz, dx)-ca;
-				crod.position.x = i->position.x+dx-i->type->connect_offset.x;
+				Vector span = crod.position+i->type->connect_offset-i->position;
+				float cd = i->type->connect_point.norm();
+				Angle ca = Geometry::atan2(i->type->connect_point.z, i->type->connect_point.x);
+				span.x = sqrt(cd*cd-span.z*span.z)*(span.x>0 ? 1 : -1);
+				i->angle = Geometry::atan2(span.z, span.x)-ca;
+				crod.position.x = i->position.x+span.x-i->type->connect_offset.x;
 			}
 			else if(i->type->limit==VehicleType::Rod::ROTATE && crod.type->limit==VehicleType::Rod::ROTATE)
 			{
-				float dx = crod.position.x-i->position.x;
-				float dz = crod.position.z-i->position.z;
-				float d = sqrt(dx*dx+dz*dz);
-				float cd1 = sqrt(i->type->connect_point.x*i->type->connect_point.x+i->type->connect_point.z*i->type->connect_point.z);
-				float cd2 = sqrt(i->type->connect_offset.x*i->type->connect_offset.x+i->type->connect_offset.z*i->type->connect_offset.z);
+				Vector span = crod.position-i->position;
+				float d = span.norm();
+				float cd1 = i->type->connect_point.norm();
+				float cd2 = i->type->connect_offset.norm();
 				float a = (d*d+cd1*cd1-cd2*cd2)/(2*d);
 				float b = sqrt(cd1*cd1-a*a);
-				float sign = (dx*i->type->connect_point.z-dz*i->type->connect_point.x>0 ? 1 : -1);
-				float cx = (dx*a-dz*b*sign)/d;
-				float cz = (dz*a+dx*b*sign)/d;
-				float ca1 = atan2(i->type->connect_point.z, i->type->connect_point.x);
-				float ca2 = atan2(i->type->connect_offset.z, i->type->connect_offset.x);
-				i->angle = atan2(cz, cx)-ca1;
-				crod.angle = atan2(cz-dz, cx-dx)-ca2;
+				float sign = (cross(i->type->connect_point, span).y>0 ? 1 : -1);
+				Vector conn = Vector(span.x*a-span.z*b, 0, span.z*a+span.x*b)/(d*sign);
+				Angle ca1 = Geometry::atan2(i->type->connect_point.z, i->type->connect_point.x);
+				Angle ca2 = Geometry::atan2(i->type->connect_offset.z, i->type->connect_offset.x);
+				i->angle = Geometry::atan2(conn.z, conn.x)-ca1;
+				crod.angle = Geometry::atan2(conn.z-span.z, conn.x-span.x)-ca2;
 			}
 		}
 	}
@@ -349,10 +344,7 @@ void Vehicle::adjust_for_distance(TrackPosition &front, TrackPosition &back, flo
 		Vector front_point = front.get_point().pos;
 		Vector 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);
+		float dist = distance(front_point, back_point);
 
 		float diff = tdist-dist;
 		if(diff<-margin && adjust_dir<=0)
@@ -375,13 +367,11 @@ void Vehicle::adjust_for_distance(TrackPosition &front, TrackPosition &back, flo
 
 TrackPoint Vehicle::get_point(const Vector &front, const Vector &back, float ratio) const
 {
-	float dx = front.x-back.x;
-	float dy = front.y-back.y;
-	float dz = front.z-back.z;
+	Vector span = front-back;
 
 	TrackPoint tp;
-	tp.pos = Vector(back.x+dx*ratio, back.y+dy*ratio, back.z+dz*ratio);
-	tp.dir = atan2(dy, dx);
+	tp.pos = back+span*ratio;
+	tp.dir = Geometry::atan2(span.y, span.x);
 
 	return tp;
 }
@@ -423,14 +413,12 @@ int Vehicle::get_link_slot(const Object &other) const
 
 
 Vehicle::Axle::Axle(const VehicleType::Axle &t):
-	type(&t),
-	angle(0)
+	type(&t)
 { }
 
 
 Vehicle::Bogie::Bogie(const VehicleType::Bogie &t):
-	type(&t),
-	direction(0)
+	type(&t)
 {
 	for(VehicleType::AxleArray::const_iterator i=type->axles.begin(); i!=type->axles.end(); ++i)
 		axles.push_back(*i);
@@ -438,8 +426,7 @@ Vehicle::Bogie::Bogie(const VehicleType::Bogie &t):
 
 
 Vehicle::Rod::Rod(const VehicleType::Rod &t):
-	type(&t),
-	angle(0)
+	type(&t)
 { }
 
 
diff --git a/source/libr2c2/vehicle.h b/source/libr2c2/vehicle.h
index bbd156c..bd815ab 100644
--- a/source/libr2c2/vehicle.h
+++ b/source/libr2c2/vehicle.h
@@ -33,7 +33,7 @@ public:
 	struct Axle
 	{
 		const VehicleType::Axle *type;
-		float angle;
+		Angle angle;
 
 		Axle(const VehicleType::Axle &);
 	};
@@ -41,7 +41,7 @@ public:
 	struct Bogie
 	{
 		const VehicleType::Bogie *type;
-		float direction;
+		Angle direction;
 		std::vector<Axle> axles;
 
 		Bogie(const VehicleType::Bogie &);
@@ -51,7 +51,7 @@ public:
 	{
 		const VehicleType::Rod *type;
 		Vector position;
-		float angle;
+		Angle angle;
 
 		Rod(const VehicleType::Rod &);
 	};
@@ -95,7 +95,7 @@ public:
 
 	// TODO implement these - should call place() with suitable parameters
 	virtual void set_position(const Vector &) { }
-	virtual void set_rotation(float) { }
+	virtual void set_rotation(const Angle &) { }
 	void place(Track &, unsigned, float, PlaceMode = CENTER);
 	void unplace();
 	void advance(float);
-- 
2.43.0