Support trains with multiple vehicles

[r2c2.git] / source / libmarklin / geometry.h

diff --git a/source/libmarklin/geometry.h b/source/libmarklin/geometry.h
index fab5cea2bd9e8bdc7d33783d258795c623ceeb43..674a6b9710ec13b640d772e3b37542c9480703c7 100644 (file)
--- a/source/libmarklin/geometry.h
+++ b/source/libmarklin/geometry.h
@@ -8,6 +8,7 @@ Distributed under the GPL
 #ifndef LIBMARKLIN_GEOMETRY_H_
 #define LIBMARKLIN_GEOMETRY_H_
 
+#include <cmath>
 #include <vector>
 
 namespace Marklin {
@@ -21,6 +22,9 @@ struct Point
        Point(float x_, float y_, float z_): x(x_), y(y_), z(z_) { }
 };
 
+inline float distance(const Point &p, const Point &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)); }
+
 struct TrackPoint
 {
        Point pos;