initial commit

src/util/geo/BezierCurve.h

@@ -0,0 +1,54 @@
+// Copyright 2016, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+#ifndef UTIL_GEO_BEZIERCURVE_H_
+#define UTIL_GEO_BEZIERCURVE_H_
+
+#include <vector>
+#include "util/geo/Geo.h"
+#include "util/geo/PolyLine.h"
+
+namespace util {
+namespace geo {
+
+struct CubicPolynom {
+  CubicPolynom(double a, double b, double c, double d, double x)
+      : a(a), b(b), c(c), d(d), x(x) {}
+  CubicPolynom() : a(0), b(0), c(0), d(0), x(0) {}
+  double a, b, c, d, x;
+
+  double valueAt(double x) const;
+};
+
+/**
+ * Bezier curve
+ */
+template <typename T>
+class BezierCurve {
+ public:
+  BezierCurve(const Point<T>& a, const Point<T>& b, const Point<T>& c, const Point<T>& d);
+
+  const PolyLine<T>& render(double d);
+
+ private:
+  double _d;
+
+  // the x and y polynoms for this spline
+  CubicPolynom _xp, _yp;
+
+  // store the rendered polyline for quicker access
+  PolyLine<T> _rendered;
+  bool _didRender;
+
+  void recalcPolynoms(const Point<T>& x, const Point<T>& b, const Point<T>& c,
+                      const Point<T>& d);
+
+  Point<T> valueAt(double t) const;
+};
+
+#include "util/geo/PolyLine.tpp"
+}
+}
+
+#endif  // UTIL_GEO_BEZIERCURVE_H_

src/util/geo/BezierCurve.tpp

@@ -0,0 +1,70 @@
+// Copyright 2016, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+// _____________________________________________________________________________
+template <typename T>
+BezierCurve<T>::BezierCurve(const Point<T>& a, const Point<T>& b, const Point<T>& c,
+                         const Point<T>& d)
+    : _d(dist(a, d)) {
+  assert(_d > 0);
+  recalcPolynoms(a, b, c, d);
+}
+
+// _____________________________________________________________________________
+template <typename T>
+void BezierCurve<T>::recalcPolynoms(const Point<T>& a, const Point<T>& b, const Point<T>& c,
+                                 const Point<T>& d) {
+  _xp.a = a.template get<0>();
+  _xp.b = 3.0 * (b.template get<0>() - a.template get<0>());
+  _xp.c = 3.0 * (c.template get<0>() - b.template get<0>()) - _xp.b;
+  _xp.d = d.template get<0>() - a.template get<0>() - _xp.c - _xp.b;
+
+  _yp.a = a.template get<1>();
+  _yp.b = 3.0 * (b.template get<1>() - a.template get<1>());
+  _yp.c = 3.0 * (c.template get<1>() - b.template get<1>()) - _yp.b;
+  _yp.d = d.template get<1>() - a.template get<1>() - _yp.c - _yp.b;
+
+  _didRender = false;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+Point<T> BezierCurve<T>::valueAt(double t) const {
+  return Point<T>(_xp.valueAt(t), _yp.valueAt(t));
+}
+
+// _____________________________________________________________________________
+template <typename T>
+const PolyLine<T>& BezierCurve<T>::render(double d) {
+  assert(d > 0);
+  if (_didRender) return _rendered;
+
+  if (_d == 0) {
+    _rendered << Point<T>(_xp.a, _yp.a) << Point<T>(_xp.a, _yp.a);
+    return _rendered;
+  }
+
+  _rendered.empty();
+  double n = _d / d, dt = 1 / n, t = 0;
+
+  bool cancel = false;
+  while (true) {
+    _rendered << valueAt(t);
+    t += dt;
+    if (cancel) break;
+    if (t > 1) {
+      t = 1;
+      cancel = true;
+    }
+  }
+
+  _didRender = true;
+  return _rendered;
+}
+
+// _____________________________________________________________________________
+double CubicPolynom::valueAt(double atx) const {
+  double dx = atx - x;
+  return a + b * dx + c * dx * dx + d * dx * dx * dx;
+}

src/util/geo/Geo.h

@@ -0,0 +1,773 @@
+// Copyright 2016, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+#ifndef UTIL_GEO_GEO_H_
+#define UTIL_GEO_GEO_H_
+
+#define _USE_MATH_DEFINES
+
+#include <math.h>
+#include <boost/geometry.hpp>
+#include <boost/geometry/strategies/transform/matrix_transformers.hpp>
+#include "util/Misc.h"
+
+// -------------------
+// Geometry stuff
+// ------------------
+
+namespace bgeo = boost::geometry;
+
+namespace util {
+namespace geo {
+
+template <typename T>
+using Point = bgeo::model::point<T, 2, bgeo::cs::cartesian>;
+
+template <typename T>
+using Line = bgeo::model::linestring<Point<T>>;
+
+template <typename T>
+using MultiLine = bgeo::model::multi_linestring<Line<T>>;
+
+template <typename T>
+using Polygon = bgeo::model::polygon<Point<T>>;
+
+template <typename T>
+using MultiPolygon = bgeo::model::multi_polygon<Polygon<T>>;
+
+template <typename T>
+using Box = bgeo::model::box<Point<T>>;
+
+// convenience aliases
+
+typedef Point<double> DPoint;
+typedef Point<float> FPoint;
+typedef Point<int> IPoint;
+
+typedef Line<double> DLine;
+typedef Line<float> FLine;
+typedef Line<int> ILine;
+
+typedef Box<double> DBox;
+typedef Box<float> FBox;
+typedef Box<int> IBox;
+
+// _____________________________________________________________________________
+template <typename T>
+inline Line<T> rotate(const Line<T>& geo, double deg, const Point<T>& center) {
+  Line<T> ret;
+
+  bgeo::strategy::transform::translate_transformer<T, 2, 2> translate(
+      -center.template get<0>(), -center.template get<1>());
+  bgeo::strategy::transform::rotate_transformer<bgeo::degree, T, 2, 2> rotate(
+      deg);
+  bgeo::strategy::transform::translate_transformer<T, 2, 2> translateBack(
+      center.template get<0>(), center.template get<1>());
+
+  bgeo::strategy::transform::ublas_transformer<T, 2, 2> translateRotate(
+      prod(rotate.matrix(), translate.matrix()));
+  bgeo::strategy::transform::ublas_transformer<T, 2, 2> all(
+      prod(translateBack.matrix(), translateRotate.matrix()));
+
+  bgeo::transform(geo, ret, all);
+
+  return ret;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline MultiLine<T> rotate(const MultiLine<T>& geo, double deg,
+                           const Point<T>& center) {
+  MultiLine<T> ret;
+
+  bgeo::strategy::transform::translate_transformer<T, 2, 2> translate(
+      -center.template get<0>(), -center.template get<1>());
+  bgeo::strategy::transform::rotate_transformer<bgeo::degree, T, 2, 2> rotate(
+      deg);
+  bgeo::strategy::transform::translate_transformer<T, 2, 2> translateBack(
+      center.template get<0>(), center.template get<1>());
+
+  bgeo::strategy::transform::ublas_transformer<T, 2, 2> translateRotate(
+      prod(rotate.matrix(), translate.matrix()));
+  bgeo::strategy::transform::ublas_transformer<T, 2, 2> all(
+      prod(translateBack.matrix(), translateRotate.matrix()));
+
+  bgeo::transform(geo, ret, all);
+
+  return ret;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline Box<T> pad(const Box<T>& box, double padding) {
+  return Box<T>(Point<T>(box.min_corner().template get<0>() - padding,
+                         box.min_corner().template get<1>() - padding),
+                Point<T>(box.max_corner().template get<0>() + padding,
+                         box.max_corner().template get<1>() + padding));
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline Line<T> rotate(const Line<T>& geo, double deg) {
+  Point<T> center;
+  bgeo::centroid(geo, center);
+  return rotate(geo, deg, center);
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline MultiLine<T> rotate(const MultiLine<T>& geo, double deg) {
+  Point<T> center;
+  bgeo::centroid(geo, center);
+  return rotate(geo, deg, center);
+}
+
+// _____________________________________________________________________________
+template <template <typename> typename Geometry, typename T>
+inline Geometry<T> move(const Geometry<T>& geo, T x, T y) {
+  Geometry<T> ret;
+  bgeo::strategy::transform::translate_transformer<T, 2, 2> translate(x, y);
+  bgeo::transform(geo, ret, translate);
+  return ret;
+}
+
+// TODO: outfactor
+
+template <typename T>
+struct RotatedBox {
+  RotatedBox(const Box<T>& b, double rot, const Point<T>& center)
+      : b(b), rotateDeg(rot), center(center) {}
+  RotatedBox(const Box<T>& b, double rot) : b(b), rotateDeg(rot) {
+    bgeo::centroid(b, center);
+  }
+
+  Box<T> b;
+  double rotateDeg;
+  Point<T> center;
+
+  Polygon<T> getPolygon() {
+    Polygon<T> hull;
+    bgeo::convex_hull(b, hull);
+    return rotate(hull, rotateDeg, center);
+  }
+};
+
+// _____________________________________________________________________________
+template <typename T>
+inline Box<T> minbox() {
+  return bgeo::make_inverse<Box<T>>();
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline RotatedBox<T> shrink(const RotatedBox<T>& b, double d) {
+  double xd =
+      b.b.max_corner().template get<0>() - b.b.min_corner().template get<0>();
+  double yd =
+      b.b.max_corner().template get<1>() - b.b.min_corner().template get<1>();
+
+  if (xd <= 2 * d) d = xd / 2 - 1;
+  if (yd <= 2 * d) d = yd / 2 - 1;
+
+  Box<T> r(Point<T>(b.b.min_corner().template get<0>() + d,
+                    b.b.min_corner().template get<1>() + d),
+           Point<T>(b.b.max_corner().template get<0>() - d,
+                    b.b.max_corner().template get<1>() - d));
+
+  return RotatedBox<T>(r, b.rotateDeg, b.center);
+}
+
+// _____________________________________________________________________________
+inline bool doubleEq(double a, double b) { return fabs(a - b) < 0.000001; }
+
+// _____________________________________________________________________________
+template <typename Geometry, typename Box>
+inline bool contains(const Geometry& geom, const Box& box) {
+  return bgeo::within(geom, box);
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline bool contains(const Point<T>& p1, const Point<T>& q1, const Point<T>& p2,
+                     const Point<T>& q2) {
+  Line<T> a, b;
+  a.push_back(p1);
+  a.push_back(q1);
+  b.push_back(p2);
+  b.push_back(q2);
+
+  return bgeo::covered_by(a, b);
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline bool contains(T p1x, T p1y, T q1x, T q1y, T p2x, T p2y, T q2x, T q2y) {
+  Point<T> p1(p1x, p1y);
+  Point<T> q1(q1x, q1y);
+  Point<T> p2(p2x, p2y);
+  Point<T> q2(q2x, q2y);
+
+  return contains(p1, q1, p2, q2);
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline bool intersects(const Point<T>& p1, const Point<T>& q1,
+                       const Point<T>& p2, const Point<T>& q2) {
+  /*
+   * checks whether two line segments intersect
+   */
+  Line<T> a, b;
+  a.push_back(p1);
+  a.push_back(q1);
+  b.push_back(p2);
+  b.push_back(q2);
+
+  return !(contains(p1, q1, p2, q2) || contains(p2, q2, p1, q1)) &&
+         bgeo::intersects(a, b);
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline bool intersects(T p1x, T p1y, T q1x, T q1y, T p2x, T p2y, T q2x, T q2y) {
+  /*
+   * checks whether two line segments intersect
+   */
+  Point<T> p1(p1x, p1y);
+  Point<T> q1(q1x, q1y);
+  Point<T> p2(p2x, p2y);
+  Point<T> q2(q2x, q2y);
+
+  return intersects(p1, q1, p2, q2);
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline Point<T> intersection(T p1x, T p1y, T q1x, T q1y, T p2x, T p2y, T q2x,
+                             T q2y) {
+  /*
+   * calculates the intersection between two line segments
+   */
+  if (doubleEq(p1x, q1x) && doubleEq(p1y, q1y))
+    return Point<T>(p1x, p1y);  // TODO: <-- intersecting with a point??
+  if (doubleEq(p2x, q1x) && doubleEq(p2y, q1y)) return Point<T>(p2x, p2y);
+  if (doubleEq(p2x, q2x) && doubleEq(p2y, q2y))
+    return Point<T>(p2x, p2y);  // TODO: <-- intersecting with a point??
+  if (doubleEq(p1x, q2x) && doubleEq(p1y, q2y)) return Point<T>(p1x, p1y);
+
+  double a = ((q2y - p2y) * (q1x - p1x)) - ((q2x - p2x) * (q1y - p1y));
+  double u = (((q2x - p2x) * (p1y - p2y)) - ((q2y - p2y) * (p1x - p2x))) / a;
+
+  return Point<T>(p1x + (q1x - p1x) * u, p1y + (q1y - p1y) * u);
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline Point<T> intersection(const Point<T>& p1, const Point<T>& q1,
+                             const Point<T>& p2, const Point<T>& q2) {
+  /*
+   * calculates the intersection between two line segments
+   */
+  return intersection(p1.template get<0>(), p1.template get<1>(),
+                      q1.template get<0>(), q1.template get<1>(),
+                      p2.template get<0>(), p2.template get<1>(),
+                      q2.template get<0>(), q2.template get<1>());
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline bool lineIntersects(T p1x, T p1y, T q1x, T q1y, T p2x, T p2y, T q2x,
+                           T q2y) {
+  /*
+   * checks whether two lines intersect
+   */
+  double EPSILON = 0.0000001;
+  double a = ((q2y - p2y) * (q1x - p1x)) - ((q2x - p2x) * (q1y - p1y));
+
+  return a > EPSILON || a < -EPSILON;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline bool lineIntersects(const Point<T>& p1, const Point<T>& q1,
+                           const Point<T>& p2, const Point<T>& q2) {
+  /*
+   * checks whether two lines intersect
+   */
+  return lineIntersects(p1.template get<0>(), p1.template get<1>(),
+                        q1.template get<0>(), q1.template get<1>(),
+                        p2.template get<0>(), p2.template get<1>(),
+                        q2.template get<0>(), q2.template get<1>());
+}
+
+// _____________________________________________________________________________
+inline double angBetween(double p1x, double p1y, double q1x, double q1y) {
+  double dY = q1y - p1y;
+  double dX = q1x - p1x;
+  return atan2(dY, dX);
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline double angBetween(const Point<T>& p1, const Point<T>& q1) {
+  return angBetween(p1.template get<0>(), p1.template get<1>(),
+                    q1.template get<0>(), q1.template get<1>());
+}
+
+// _____________________________________________________________________________
+inline double dist(double x1, double y1, double x2, double y2) {
+  return sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
+}
+
+// _____________________________________________________________________________
+inline double innerProd(double x1, double y1, double x2, double y2, double x3,
+                        double y3) {
+  double dx21 = x2 - x1;
+  double dx31 = x3 - x1;
+  double dy21 = y2 - y1;
+  double dy31 = y3 - y1;
+  double m12 = sqrt(dx21 * dx21 + dy21 * dy21);
+  double m13 = sqrt(dx31 * dx31 + dy31 * dy31);
+  double theta = acos(std::min((dx21 * dx31 + dy21 * dy31) / (m12 * m13), 1.0));
+
+  return theta * (180 / M_PI);
+}
+
+// _____________________________________________________________________________
+template <typename G>
+inline double innerProd(const G& a, const G& b, const G& c) {
+  return innerProd(a.template get<0>(), a.template get<1>(),
+                   b.template get<0>(), b.template get<1>(),
+                   c.template get<0>(), c.template get<1>());
+}
+
+// _____________________________________________________________________________
+template <typename GeometryA, typename GeometryB>
+inline double dist(const GeometryA& p1, const GeometryB& p2) {
+  return bgeo::distance(p1, p2);
+}
+
+// _____________________________________________________________________________
+template <typename Geometry>
+inline std::string getWKT(Geometry g) {
+  std::stringstream ss;
+  ss << bgeo::wkt(g);
+  return ss.str();
+}
+
+// _____________________________________________________________________________
+template <typename Geometry>
+inline double len(Geometry g) {
+  return bgeo::length(g);
+}
+
+// _____________________________________________________________________________
+template <typename Geometry>
+inline Geometry simplify(Geometry g, double d) {
+  Geometry ret;
+  bgeo::simplify(g, ret, d);
+  return ret;
+}
+
+// _____________________________________________________________________________
+inline double distToSegment(double lax, double lay, double lbx, double lby,
+                            double px, double py) {
+  double d = dist(lax, lay, lbx, lby) * dist(lax, lay, lbx, lby);
+  if (d == 0) return dist(px, py, lax, lay);
+
+  double t = ((px - lax) * (lbx - lax) + (py - lay) * (lby - lay)) / d;
+
+  if (t < 0) {
+    return dist(px, py, lax, lay);
+  } else if (t > 1) {
+    return dist(px, py, lbx, lby);
+  }
+
+  return dist(px, py, lax + t * (lbx - lax), lay + t * (lby - lay));
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline double distToSegment(const Point<T>& la, const Point<T>& lb,
+                            const Point<T>& p) {
+  return distToSegment(la.template get<0>(), la.template get<1>(),
+                       lb.template get<0>(), lb.template get<1>(),
+                       p.template get<0>(), p.template get<1>());
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline Point<T> projectOn(const Point<T>& a, const Point<T>& b,
+                          const Point<T>& c) {
+  if (doubleEq(a.template get<0>(), b.template get<0>()) &&
+      doubleEq(a.template get<1>(), b.template get<1>()))
+    return a;
+  if (doubleEq(a.template get<0>(), c.template get<0>()) &&
+      doubleEq(a.template get<1>(), c.template get<1>()))
+    return a;
+  if (doubleEq(b.template get<0>(), c.template get<0>()) &&
+      doubleEq(b.template get<1>(), c.template get<1>()))
+    return b;
+
+  double x, y;
+
+  if (c.template get<0>() == a.template get<0>()) {
+    // infinite slope
+    x = a.template get<0>();
+    y = b.template get<1>();
+  } else {
+    double m = (double)(c.template get<1>() - a.template get<1>()) /
+               (c.template get<0>() - a.template get<0>());
+    double bb = (double)a.template get<1>() - (m * a.template get<0>());
+
+    x = (m * b.template get<1>() + b.template get<0>() - m * bb) / (m * m + 1);
+    y = (m * m * b.template get<1>() + m * b.template get<0>() + bb) /
+        (m * m + 1);
+  }
+
+  Point<T> ret = Point<T>(x, y);
+
+  bool isBetween = dist(a, c) > dist(a, ret) && dist(a, c) > dist(c, ret);
+  bool nearer = dist(a, ret) < dist(c, ret);
+
+  if (!isBetween) return nearer ? a : c;
+
+  return ret;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline double parallelity(const Box<T>& box, const Line<T>& line) {
+  double ret = M_PI;
+
+  double a = angBetween(box.min_corner(),
+                        Point<T>(box.min_corner().template get<0>(),
+                                 box.max_corner().template get<1>()));
+  double b = angBetween(box.min_corner(),
+                        Point<T>(box.max_corner().template get<0>(),
+                                 box.min_corner().template get<1>()));
+  double c = angBetween(box.max_corner(),
+                        Point<T>(box.min_corner().template get<0>(),
+                                 box.max_corner().template get<1>()));
+  double d = angBetween(box.max_corner(),
+                        Point<T>(box.max_corner().template get<0>(),
+                                 box.min_corner().template get<1>()));
+
+  double e = angBetween(line.front(), line.back());
+
+  double vals[] = {a, b, c, d};
+
+  for (double ang : vals) {
+    double v = fabs(ang - e);
+    if (v > M_PI) v = 2 * M_PI - v;
+    if (v > M_PI / 2) v = M_PI - v;
+    if (v < ret) ret = v;
+  }
+
+  return 1 - (ret / (M_PI / 4));
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline double parallelity(const Box<T>& box, const MultiLine<T>& multiline) {
+  double ret = 0;
+  for (const Line<T>& l : multiline) {
+    ret += parallelity(box, l);
+  }
+
+  return ret / multiline.size();
+}
+
+// _____________________________________________________________________________
+template <typename GeomA, typename GeomB>
+inline bool intersects(const GeomA& a, const GeomB& b) {
+  return bgeo::intersects(a, b);
+}
+
+// _____________________________________________________________________________
+template <typename T, template <typename> typename Geometry>
+inline RotatedBox<T> getOrientedEnvelope(Geometry<T> pol) {
+  // TODO: implement this nicer, works for now, but inefficient
+  // see
+  // https://geidav.wordpress.com/tag/gift-wrapping/#fn-1057-FreemanShapira1975
+  // for a nicer algorithm
+
+  Point<T> center;
+  bgeo::centroid(pol, center);
+
+  Box<T> tmpBox = getBoundingBox(pol);
+  double rotateDeg = 0;
+
+  // rotate in 5 deg steps
+  for (int i = 1; i < 360; i += 1) {
+    pol = rotate(pol, 1, center);
+    Box<T> e;
+    bgeo::envelope(pol, e);
+    if (bgeo::area(tmpBox) > bgeo::area(e)) {
+      tmpBox = e;
+      rotateDeg = i;
+    }
+  }
+
+  return RotatedBox<T>(tmpBox, -rotateDeg, center);
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline Box<T> getBoundingBox(Point<T> pol) {
+  Box<T> tmpBox;
+  bgeo::envelope(pol, tmpBox);
+  return tmpBox;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline Box<T> getBoundingBox(Line<T> pol) {
+  Box<T> tmpBox;
+  bgeo::envelope(pol, tmpBox);
+  return tmpBox;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline Box<T> getBoundingBox(Polygon<T> pol) {
+  Box<T> tmpBox;
+  bgeo::envelope(pol, tmpBox);
+  return tmpBox;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline Box<T> extendBox(const Box<T>& a, Box<T> b) {
+  bgeo::expand(b, a);
+  return b;
+}
+
+// _____________________________________________________________________________
+template <typename G, typename T>
+inline Box<T> extendBox(G pol, Box<T> b) {
+  Box<T> tmp;
+  bgeo::envelope(pol, tmp);
+  bgeo::expand(b, tmp);
+  return b;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline double commonArea(const Box<T>& ba, const Box<T>& bb) {
+  T l = std::max(ba.min_corner().template get<0>(),
+                 bb.min_corner().template get<0>());
+  T r = std::min(ba.max_corner().template get<0>(),
+                 bb.max_corner().template get<0>());
+  T b = std::max(ba.min_corner().template get<1>(),
+                 bb.min_corner().template get<1>());
+  T t = std::min(ba.max_corner().template get<1>(),
+                 bb.max_corner().template get<1>());
+
+  if (l > r || b > t) return 0;
+
+  return (r - l) * (t - b);
+}
+
+// _____________________________________________________________________________
+template <typename T, template <typename> typename Geometry>
+inline RotatedBox<T> getFullEnvelope(Geometry<T> pol) {
+  Point<T> center;
+  bgeo::centroid(pol, center);
+
+  Box<T> tmpBox;
+  bgeo::envelope(pol, tmpBox);
+  double rotateDeg = 0;
+
+  MultiPolygon<T> ml;
+
+  // rotate in 5 deg steps
+  for (int i = 1; i < 360; i += 1) {
+    pol = rotate(pol, 1, center);
+    Polygon<T> hull;
+    bgeo::convex_hull(pol, hull);
+    ml.push_back(hull);
+    Box<T> e;
+    bgeo::envelope(pol, e);
+    if (bgeo::area(tmpBox) > bgeo::area(e)) {
+      tmpBox = e;
+      rotateDeg = i;
+    }
+  }
+
+  bgeo::envelope(ml, tmpBox);
+
+  return RotatedBox<T>(tmpBox, rotateDeg, center);
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline RotatedBox<T> getOrientedEnvelopeAvg(MultiLine<T> ml) {
+  MultiLine<T> orig = ml;
+  // get oriented envelope for hull
+  RotatedBox<T> rbox = getFullEnvelope(ml);
+  Point<T> center;
+  bgeo::centroid(rbox.b, center);
+
+  ml = rotate(ml, -rbox.rotateDeg - 45, center);
+
+  double bestDeg = -45;
+  double score = parallelity(rbox.b, ml);
+
+  for (double i = -45; i <= 45; i += .5) {
+    ml = rotate(ml, -.5, center);
+    double p = parallelity(rbox.b, ml);
+    if (parallelity(rbox.b, ml) > score) {
+      bestDeg = i;
+      score = p;
+    }
+  }
+
+  rbox.rotateDeg += bestDeg;
+
+  // move the box along 45deg angles from its origin until it fits the ml
+  // = until the intersection of its hull and the box is largest
+  Polygon<T> p = rbox.getPolygon();
+  p = rotate(p, -rbox.rotateDeg, rbox.center);
+
+  Polygon<T> hull;
+  bgeo::convex_hull(orig, hull);
+  hull = rotate(hull, -rbox.rotateDeg, rbox.center);
+
+  Box<T> box;
+  bgeo::envelope(hull, box);
+  rbox = RotatedBox<T>(box, rbox.rotateDeg, rbox.center);
+
+  return rbox;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline Line<T> densify(const Line<T>& l, double d) {
+  if (!l.size()) return l;
+
+  Line<T> ret;
+  ret.reserve(l.size());
+  ret.push_back(l.front());
+
+  for (size_t i = 1; i < l.size(); i++) {
+    double segd = dist(l[i-1], l[i]);
+    double dx =
+        (l[i].template get<0>() - l[i-1].template get<0>()) / segd;
+    double dy =
+        (l[i].template get<1>() - l[i-1].template get<1>()) / segd;
+    double curd = d;
+    while (curd < segd) {
+      ret.push_back(Point<T>(l[i-1].template get<0>() + dx * curd,
+                             l[i-1].template get<1>() + dy * curd));
+      curd += d;
+    }
+
+    ret.push_back(l[i]);
+  }
+
+  return ret;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline double frechetDistC(size_t i, size_t j, const Line<T>& p,
+                           const Line<T>& q,
+                           std::vector<std::vector<double>>& ca) {
+  // based on Eiter / Mannila
+  // http://www.kr.tuwien.ac.at/staff/eiter/et-archive/cdtr9464.pdf
+
+  if (ca[i][j] > -1)
+    return ca[i][j];
+  else if (i == 0 && j == 0)
+    ca[i][j] = dist(p[0], q[0]);
+  else if (i > 0 && j == 0)
+    ca[i][j] = std::max(frechetDistC(i - 1, 0, p, q, ca), dist(p[i], q[0]));
+  else if (i == 0 && j > 0)
+    ca[i][j] = std::max(frechetDistC(0, j - 1, p, q, ca), dist(p[0], q[j]));
+  else if (i > 0 && j > 0)
+    ca[i][j] = std::max(std::min(std::min(frechetDistC(i - 1, j, p, q, ca),
+                                          frechetDistC(i - 1, j - 1, p, q, ca)),
+                                 frechetDistC(i, j - 1, p, q, ca)),
+                        dist(p[i], q[j]));
+  else
+    ca[i][j] = std::numeric_limits<double>::infinity();
+
+  return ca[i][j];
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline double frechetDist(const Line<T>& a, const Line<T>& b, double d) {
+  // based on Eiter / Mannila
+  // http://www.kr.tuwien.ac.at/staff/eiter/et-archive/cdtr9464.pdf
+
+  auto p = densify(a, d);
+  auto q = densify(b, d);
+
+  std::vector<std::vector<double>> ca(p.size(),
+                                      std::vector<double>(q.size(), -1.0));
+  double fd = frechetDistC(p.size() - 1, q.size() - 1, p, q, ca);
+
+  return fd;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline double accFrechetDistC(const Line<T>& a, const Line<T>& b, double d) {
+
+  auto p = densify(a, d);
+  auto q = densify(b, d);
+
+  std::vector<std::vector<double>> ca(p.size(),
+                                        std::vector<double>(q.size(), 0));
+
+  for (size_t i = 0; i < p.size(); i++) ca[i][0] = std::numeric_limits<double>::infinity();
+  for (size_t j = 0; j < q.size(); j++) ca[0][j] = std::numeric_limits<double>::infinity();
+  ca[0][0] = 0;
+
+  for (size_t i = 1; i < p.size(); i++) {
+    for (size_t j = 1; j < q.size(); j++) {
+      double d = util::geo::dist(p[i], q[j]) * util::geo::dist(p[i], p[i-1]);
+      ca[i][j] = d + std::min(ca[i-1][j], std::min(ca[i][j-1], ca[i-1][j-1]));
+    }
+  }
+
+  return ca[p.size() - 1][q.size() - 1];
+}
+
+
+
+// _____________________________________________________________________________
+template <typename T>
+inline Point<T> latLngToWebMerc(double lat, double lng) {
+  double x = 6378137.0 * lng * 0.017453292519943295;
+  double a = lat * 0.017453292519943295;
+
+  double y = 3189068.5 * log((1.0 + sin(a)) / (1.0 - sin(a)));
+  return Point<T>(x, y);
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline Point<T> webMercToLatLng(double x, double y) {
+  double lat = 114.591559026 * (atan(exp(y / 6378137.0)) - 0.78539825);
+  double lon = x / 111319.4907932735677;
+  return Point<T>(lon, lat);
+}
+
+// _____________________________________________________________________________
+template <typename G1, typename G2>
+inline double webMercMeterDist(const G1& a, const G2& b) {
+  // euclidean distance on web mercator is in meters on equator,
+  // and proportional to cos(lat) in both y directions
+
+  double latA = 2 * atan(exp(a.template get<1>() / 6378137.0)) - 1.5707965;
+  double latB = 2 * atan(exp(b.template get<1>() / 6378137.0)) - 1.5707965;
+
+  return util::geo::dist(a, b) * cos((latA + latB) / 2.0);
+}
+}
+}
+
+#endif  // UTIL_GEO_GEO_H_

src/util/geo/GeoGraph.h

@@ -0,0 +1,31 @@
+// Copyright 2016, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+#ifndef UTIL_GEOGRAPH_H_
+#define UTIL_GEOGRAPH_H_
+
+#include <map>
+#include "util/geo/Geo.h"
+
+namespace util {
+namespace geograph {
+
+template<typename T>
+class GeoEdgePL {
+ public:
+  virtual const util::geo::Line<T>* getGeom() const = 0;
+  virtual void getAttrs(std::map<std::string, std::string>* m) const = 0;
+};
+
+template<typename T>
+class GeoNodePL {
+ public:
+  virtual const util::geo::Point<T>* getGeom() const = 0;
+  virtual void getAttrs(std::map<std::string, std::string>* m) const = 0;
+};
+
+}  // namespace geograph
+}  // namespace util
+
+#endif  // UTIL_GEOGRAPH_H_

src/util/geo/Grid.h

@@ -0,0 +1,88 @@
+// Copyright 2016, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Author: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+#ifndef UTIL_GEO_GRID_H_
+#define UTIL_GEO_GRID_H_
+
+#include <set>
+#include <vector>
+#include "util/geo/Geo.h"
+
+namespace util {
+namespace geo {
+
+class GridException : public std::runtime_error {
+ public:
+  GridException(std::string const& msg) : std::runtime_error(msg) {}
+};
+
+template <typename V, template <typename> typename G, typename T>
+class Grid {
+ public:
+  // initialization of a point grid with cell width w and cell height h
+  // that covers the area of bounding box bbox
+  Grid(double w, double h, const Box<T>& bbox);
+
+  // initialization of a point grid with cell width w and cell height h
+  // that covers the area of bounding box bbox
+  // optional parameters specifies whether a value->cell index
+  // should be kept (true by default!)
+  Grid(double w, double h, const Box<T>& bbox, bool buildValIdx);
+
+  // the empty grid
+  Grid();
+  // the empty grid
+  Grid(bool buildValIdx);
+
+  // add object t to this grid
+  void add(G<T> geom, V val);
+  void add(size_t x, size_t y, V val);
+
+  void get(const Box<T>& btbox, std::set<V>* s) const;
+  void get(const G<T>& geom, double d, std::set<V>* s) const;
+  void get(size_t x, size_t y, std::set<V>* s) const;
+  void remove(V val);
+
+  void getNeighbors(const V& val, double d, std::set<V>* s) const;
+  void getCellNeighbors(const V& val, size_t d, std::set<V>* s) const;
+  void getCellNeighbors(size_t x, size_t y, size_t xPerm, size_t yPerm,
+                        std::set<V>* s) const;
+
+  std::set<std::pair<size_t, size_t> > getCells(const V& val) const;
+
+  size_t getXWidth() const;
+  size_t getYHeight() const;
+
+ private:
+  double _width;
+  double _height;
+
+  double _cellWidth;
+  double _cellHeight;
+
+  Box<T> _bb;
+
+  size_t _counter;
+
+  size_t _xWidth;
+  size_t _yHeight;
+
+  bool _hasValIdx;
+
+  std::vector<std::vector<std::set<V> > > _grid;
+  std::map<V, std::set<std::pair<size_t, size_t> > > _index;
+  std::set<V> _removed;
+
+  Box<T> getBox(size_t x, size_t y) const;
+
+  size_t getCellXFromX(double lon) const;
+  size_t getCellYFromY(double lat) const;
+};
+
+#include "util/geo/Grid.tpp"
+
+}  // namespace geo
+}  // namespace util
+
+#endif  // UTIL_GEO_GRID_H_

src/util/geo/Grid.tpp

@@ -0,0 +1,228 @@
+// Copyright 2017, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Author: Patrick Brosi <brosip@informatik.uni-freiburg.de>
+
+// _____________________________________________________________________________
+template <typename V, template <typename> typename G, typename T>
+Grid<V, G, T>::Grid(bool bldIdx)
+    : _width(0),
+      _height(0),
+      _cellWidth(0),
+      _cellHeight(0),
+      _xWidth(0),
+      _yHeight(0),
+      _hasValIdx(bldIdx) {}
+
+// _____________________________________________________________________________
+template <typename V, template <typename> typename G, typename T>
+Grid<V, G, T>::Grid() : Grid<V, G, T>(true) {}
+
+// _____________________________________________________________________________
+template <typename V, template <typename> typename G, typename T>
+Grid<V, G, T>::Grid(double w, double h, const Box<T>& bbox)
+    : Grid<V, G, T>(w, h, bbox, true) {}
+
+// _____________________________________________________________________________
+template <typename V, template <typename> typename G, typename T>
+Grid<V, G, T>::Grid(double w, double h, const Box<T>& bbox, bool bValIdx)
+    : _cellWidth(fabs(w)),
+      _cellHeight(fabs(h)),
+      _bb(bbox),
+      _hasValIdx(bValIdx) {
+  _width =
+      bbox.max_corner().template get<0>() - bbox.min_corner().template get<0>();
+  _height =
+      bbox.max_corner().template get<1>() - bbox.min_corner().template get<1>();
+
+  if (_width < 0 || _height < 0) {
+    _width = 0;
+    _height = 0;
+    _xWidth = 0;
+    _yHeight = 0;
+    return;
+  }
+
+  _xWidth = ceil(_width / _cellWidth);
+  _yHeight = ceil(_height / _cellHeight);
+
+  // resize rows
+  _grid.resize(_xWidth);
+
+  // resize columns
+  for (size_t i = 0; i < _xWidth; i++) {
+    _grid[i].resize(_yHeight);
+  }
+}
+
+// _____________________________________________________________________________
+template <typename V, template <typename> typename G, typename T>
+void Grid<V, G, T>::add(G<T> geom, V val) {
+  Box<T> box = getBoundingBox(geom);
+  size_t swX = getCellXFromX(box.min_corner().template get<0>());
+  size_t swY = getCellYFromY(box.min_corner().template get<1>());
+
+  size_t neX = getCellXFromX(box.max_corner().template get<0>());
+  size_t neY = getCellYFromY(box.max_corner().template get<1>());
+
+  for (size_t x = swX; x <= neX && x < _grid.size(); x++) {
+    for (size_t y = swY; y <= neY && y < _grid[x].size(); y++) {
+      if (intersects(geom, getBox(x, y))) {
+        add(x, y, val);
+      }
+    }
+  }
+}
+
+// _____________________________________________________________________________
+template <typename V, template <typename> typename G, typename T>
+void Grid<V, G, T>::add(size_t x, size_t y, V val) {
+  _grid[x][y].insert(val);
+  if (_hasValIdx) _index[val].insert(std::pair<size_t, size_t>(x, y));
+}
+
+// _____________________________________________________________________________
+template <typename V, template <typename> typename G, typename T>
+void Grid<V, G, T>::get(const Box<T>& box, std::set<V>* s) const {
+  size_t swX = getCellXFromX(box.min_corner().template get<0>());
+  size_t swY = getCellYFromY(box.min_corner().template get<1>());
+
+  size_t neX = getCellXFromX(box.max_corner().template get<0>());
+  size_t neY = getCellYFromY(box.max_corner().template get<1>());
+
+  for (size_t x = swX; x <= neX && x >= 0 && x < _xWidth; x++)
+    for (size_t y = swY; y <= neY && y >= 0 && y < _yHeight; y++) get(x, y, s);
+}
+
+// _____________________________________________________________________________
+template <typename V, template <typename> typename G, typename T>
+void Grid<V, G, T>::get(const G<T>& geom, double d, std::set<V>* s) const {
+  Box<T> a = getBoundingBox(geom);
+  Box<T> b(Point<T>(a.min_corner().template get<0>() - d,
+                    a.min_corner().template get<1>() - d),
+           Point<T>(a.max_corner().template get<0>() + d,
+                    a.max_corner().template get<1>() + d));
+  return get(b, s);
+}
+
+// _____________________________________________________________________________
+template <typename V, template <typename> typename G, typename T>
+void Grid<V, G, T>::get(size_t x, size_t y, std::set<V>* s) const {
+  if (_hasValIdx) {
+    s->insert(_grid[x][y].begin(), _grid[x][y].end());
+  } else {
+    // if we dont have a value index, we have a set of deleted nodes.
+    // in this case, only insert if not deleted
+    std::copy_if(_grid[x][y].begin(), _grid[x][y].end(),
+                 std::inserter(*s, s->end()),
+                 [&](const V& v) { return Grid<V, G, T>::_removed.count(v) == 0; });
+  }
+}
+
+// _____________________________________________________________________________
+template <typename V, template <typename> typename G, typename T>
+void Grid<V, G, T>::remove(V val) {
+  if (_hasValIdx) {
+    auto i = _index.find(val);
+    if (i == _index.end()) return;
+
+    for (auto pair : i->second) {
+      _grid[pair.first][pair.second].erase(
+          _grid[pair.first][pair.second].find(val));
+    }
+
+    _index.erase(i);
+  } else {
+    _removed.insert(val);
+  }
+}
+
+// _____________________________________________________________________________
+template <typename V, template <typename> typename G, typename T>
+void Grid<V, G, T>::getNeighbors(const V& val, double d, std::set<V>* s) const {
+  if (!_hasValIdx) throw GridException("No value index build!");
+  auto it = _index.find(val);
+  if (it == _index.end()) return;
+
+  size_t xPerm = ceil(d / _cellWidth);
+  size_t yPerm = ceil(d / _cellHeight);
+
+  for (auto pair : it->second) {
+    getCellNeighbors(pair.first, pair.second, xPerm, yPerm, s);
+  }
+}
+
+// _____________________________________________________________________________
+template <typename V, template <typename> typename G, typename T>
+void Grid<V, G, T>::getCellNeighbors(const V& val, size_t d,
+                                     std::set<V>* s) const {
+  if (!_hasValIdx) throw GridException("No value index build!");
+  auto it = _index.find(val);
+  if (it == _index.end()) return;
+
+  for (auto pair : it->second) {
+    getCellNeighbors(pair.first, pair.second, d, d, s);
+  }
+}
+
+// _____________________________________________________________________________
+template <typename V, template <typename> typename G, typename T>
+void Grid<V, G, T>::getCellNeighbors(size_t cx, size_t cy, size_t xPerm,
+                                     size_t yPerm, std::set<V>* s) const {
+  size_t swX = xPerm > cx ? 0 : cx - xPerm;
+  size_t swY = yPerm > cy ? 0 : cy - yPerm;
+
+  size_t neX = xPerm + cx + 1 > _xWidth ? _xWidth : cx + xPerm + 1;
+  size_t neY = yPerm + cy + 1 > _yHeight ? _yHeight : cy + yPerm + 1;
+
+  for (size_t x = swX; x < neX; x++) {
+    for (size_t y = swY; y < neY; y++) {
+      s->insert(_grid[x][y].begin(), _grid[x][y].end());
+    }
+  }
+}
+
+// _____________________________________________________________________________
+template <typename V, template <typename> typename G, typename T>
+std::set<std::pair<size_t, size_t> > Grid<V, G, T>::getCells(
+    const V& val) const {
+  if (!_hasValIdx) throw GridException("No value index build!");
+  return _index.find(val)->second;
+}
+
+// _____________________________________________________________________________
+template <typename V, template <typename> typename G, typename T>
+Box<T> Grid<V, G, T>::getBox(size_t x, size_t y) const {
+  Point<T> sw(_bb.min_corner().template get<0>() + x * _cellWidth,
+              _bb.min_corner().template get<1>() + y * _cellHeight);
+  Point<T> ne(_bb.min_corner().template get<0>() + (x + 1) * _cellWidth,
+              _bb.min_corner().template get<1>() + (y + 1) * _cellHeight);
+  return Box<T>(sw, ne);
+}
+
+// _____________________________________________________________________________
+template <typename V, template <typename> typename G, typename T>
+size_t Grid<V, G, T>::getCellXFromX(double x) const {
+  float dist = x - _bb.min_corner().template get<0>();
+  if (dist < 0) dist = 0;
+  return floor(dist / _cellWidth);
+}
+
+// _____________________________________________________________________________
+template <typename V, template <typename> typename G, typename T>
+size_t Grid<V, G, T>::getCellYFromY(double y) const {
+  float dist = y - _bb.min_corner().template get<1>();
+  if (dist < 0) dist = 0;
+  return floor(dist / _cellHeight);
+}
+
+// _____________________________________________________________________________
+template <typename V, template <typename> typename G, typename T>
+size_t Grid<V, G, T>::getXWidth() const {
+  return _xWidth;
+}
+
+// _____________________________________________________________________________
+template <typename V, template <typename> typename G, typename T>
+size_t Grid<V, G, T>::getYHeight() const {
+  return _yHeight;
+}

src/util/geo/PolyLine.h

@@ -0,0 +1,139 @@
+// Copyright 2016, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+#ifndef UTIL_GEO_POLYLINE_H_
+#define UTIL_GEO_POLYLINE_H_
+
+#include <ostream>
+#include <string>
+#include <vector>
+#include "Geo.h"
+
+namespace util {
+namespace geo {
+
+static const double MAX_EQ_DISTANCE = 15;
+static const double AVERAGING_STEP = 20;
+
+template <typename T>
+struct LinePoint {
+  LinePoint() : lastIndex(0), totalPos(-1), p() {}
+
+  LinePoint(size_t i, double pos, const Point<T>& p)
+      : lastIndex(i), totalPos(pos), p(p) {}
+  size_t lastIndex;
+  double totalPos;
+  Point<T> p;
+};
+
+template <typename T>
+struct LinePointCmp {
+  bool operator()(const LinePoint<T>& lh, const LinePoint<T>& rh) const {
+    return lh.totalPos < rh.totalPos;
+  }
+};
+
+
+template <typename T>
+using LinePointPair = std::pair<LinePoint<T>, LinePoint<T>>;
+template <typename T>
+using SharedSegment = std::pair<LinePointPair<T>, LinePointPair<T>>;
+
+template <typename T>
+struct SharedSegments {
+  std::vector<SharedSegment<T>> segments;
+};
+
+// TODO: maybe let this class inherit from a more generic geometry class
+template <typename T>
+class PolyLine {
+ public:
+  PolyLine();
+  PolyLine(const Point<T>& from, const Point<T>& to);
+  PolyLine(const Line<T>& l);
+
+  PolyLine& operator<<(const Point<T>& p);
+  PolyLine& operator>>(const Point<T>& p);
+
+  void reverse();
+  PolyLine getReversed() const;
+
+  void offsetPerp(double units);
+
+  PolyLine getPerpOffsetted(double units) const;
+
+  const Line<T>& getLine() const;
+
+  double distTo(const PolyLine<T>& g) const;
+  double distTo(const Point<T>& p) const;
+
+  SharedSegments<T> getSharedSegments(const PolyLine<T>& pl, double dmax) const;
+
+  double getLength() const;
+
+  // return point at dist
+  LinePoint<T> getPointAtDist(double dist) const;
+
+  // return point at [0..1]
+  LinePoint<T> getPointAt(double dist) const;
+
+  PolyLine<T> getSegment(double a, double b) const;
+  PolyLine<T> getSegmentAtDist(double dista, double distb) const;
+  PolyLine<T> getSegment(const LinePoint<T>& start, const LinePoint<T>& end) const;
+  PolyLine<T> getSegment(const Point<T>& a, const Point<T>& b) const;
+
+  std::set<LinePoint<T>, LinePointCmp<T>> getIntersections(const PolyLine<T>& g) const;
+
+  static PolyLine<T> average(const std::vector<const PolyLine<T>*>& lines);
+  static PolyLine<T> average(const std::vector<const PolyLine<T>*>& lines,
+                          const std::vector<double>& weights);
+
+  void simplify(double d);
+  void empty();
+
+  void smoothenOutliers(double d);
+
+  std::pair<size_t, double> nearestSegment(const Point<T>& p) const;
+  std::pair<size_t, double> nearestSegmentAfter(const Point<T>& p,
+                                                size_t after) const;
+
+  LinePoint<T> projectOn(const Point<T>& p) const;
+  LinePoint<T> projectOnAfter(const Point<T>& p, size_t after) const;
+
+  void move(double vx, double vy);
+
+  std::pair<double, double> getSlopeBetween(double ad, double bd) const;
+  std::pair<double, double> getSlopeBetweenDists(double ad, double bd) const;
+
+  // equality operator, will hold frechet-distance equality check in
+  // the dmax
+  bool operator==(const PolyLine& rhs) const;
+  bool contains(const PolyLine& rhs, double dmax) const;
+  bool equals(const PolyLine& rhs) const;
+  bool equals(const PolyLine& rhs, double dmax) const;
+
+  std::string getWKT() const;
+
+  PolyLine getOrthoLineAtDist(double d, double lengt) const;
+
+  Point<T> interpolate(const Point<T>& a, const Point<T>& b, double p) const;
+
+  void fixTopology(double maxl);
+  void applyChaikinSmooth(size_t depth);
+
+  const Point<T>& front() const;
+  const Point<T>& back() const;
+
+ private:
+  std::set<LinePoint<T>, LinePointCmp<T>> getIntersections(const PolyLine& p,
+                                                     size_t a, size_t b) const;
+  Line<T> _line;
+};
+
+#include "util/geo/PolyLine.tpp"
+
+}  // namespace geo
+}  // namespace util
+
+#endif  // UTIL_GEO_POLYLINE_H_

src/util/geo/PolyLine.tpp

@@ -0,0 +1,739 @@
+// Copyright 2016, University of Freibur
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+#include "util/geo/Geo.h"
+#include "util/geo/PolyLine.h"
+
+// _____________________________________________________________________________
+template <typename T>
+PolyLine<T>::PolyLine() {}
+
+// _____________________________________________________________________________
+template <typename T>
+PolyLine<T>::PolyLine(const Point<T>& from, const Point<T>& to) {
+  *this << from << to;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+PolyLine<T>::PolyLine(const Line<T>& l) : _line(l) {}
+
+// _____________________________________________________________________________
+template <typename T>
+PolyLine<T>& PolyLine<T>::operator<<(const Point<T>& p) {
+  _line.push_back(p);
+  return *this;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+PolyLine<T>& PolyLine<T>::operator>>(const Point<T>& p) {
+  _line.insert(_line.begin(), p);
+  return *this;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+void PolyLine<T>::reverse() {
+  std::reverse(_line.begin(), _line.end());
+}
+
+// _____________________________________________________________________________
+template <typename T>
+PolyLine<T> PolyLine<T>::getReversed() const {
+  PolyLine ret = *this;
+  ret.reverse();
+  return ret;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+const Line<T>& PolyLine<T>::getLine() const {
+  return _line;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+PolyLine<T> PolyLine<T>::getPerpOffsetted(double units) const {
+  PolyLine p = *this;
+  p.offsetPerp(units);
+  return p;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+void PolyLine<T>::offsetPerp(double units) {
+  /*
+   * calculate perpendicular offset of a polyline
+   *
+   * there doesn't seem to be any library which reliably does that,
+   * so we do it ourself here until we find one...
+   * boost::geometry only supports buffering a line, resulting in a
+   * polygon. An offsetted line is part of that polygon, but retrieving
+   * it reliably could result in some geometrical diffing hocus pocus which is
+   * bound to go wrong at /some/ point (self intersections, numerical
+   * instability etc)
+   */
+
+  if (fabs(units) < 0.001) return;
+
+  assert(getLength() > 0);
+
+  if (_line.size() < 2) return;
+
+  Line<T> ret;
+  Point<T> lastP = _line.front();
+
+  Point<T> *lastIns = 0, *befLastIns = 0;
+
+  for (size_t i = 1; i < _line.size(); i++) {
+    Point<T> curP = _line[i];
+
+    double n1 = lastP.template get<1>() - curP.template get<1>();
+    double n2 = curP.template get<0>() - lastP.template get<0>();
+    double n = sqrt(n1 * n1 + n2 * n2);
+
+    // if n == 0, the segment is effectively a point
+    // we would get into all sorts of troubles if we tried to offset a point...
+    if (!(n > 0)) continue;
+
+    n1 = n1 / n;
+    n2 = n2 / n;
+
+    lastP.set<0>(lastP.template get<0>() + (n1 * units));
+    lastP.set<1>(lastP.template get<1>() + (n2 * units));
+
+    curP.set<0>(curP.template get<0>() + (n1 * units));
+    curP.set<1>(curP.template get<1>() + (n2 * units));
+
+    if (lastIns && befLastIns &&
+        lineIntersects(*lastIns, *befLastIns, lastP, curP)) {
+      *lastIns = intersection(*lastIns, *befLastIns, lastP, curP);
+
+      double d = dist(lastP, *lastIns);
+      double d2 = distToSegment(*lastIns, *befLastIns, lastP);
+
+      if (d > fabs(units) * 2 && d2 < d - (fabs(units))) {
+        PolyLine pl(*lastIns, *befLastIns);
+        PolyLine pll(*lastIns, curP);
+        pl = pl.getSegment(0, (d - (fabs(units))) / pl.getLength());
+        pll = pll.getSegment(0, (d - (fabs(units))) / pll.getLength());
+
+        ret.push_back(pll.back());
+        *lastIns = pl.back();
+
+        ret.push_back(curP);
+      } else {
+        ret.push_back(curP);
+      }
+    } else {
+      ret.push_back(lastP);
+      ret.push_back(curP);
+    }
+
+    lastIns = &ret[ret.size() - 1];
+    befLastIns = &ret[ret.size() - 2];
+
+    lastP = _line[i];
+  }
+
+  _line = ret;
+
+  // heuristics
+  simplify(1);
+  fixTopology(fabs(2 * 3.14 * units));
+}
+
+// _____________________________________________________________________________
+template <typename T>
+PolyLine<T> PolyLine<T>::getSegment(double a, double b) const {
+  if (a > b) {
+    double c = a;
+    a = b;
+    b = c;
+  }
+  LinePoint<T> start = getPointAt(a);
+  LinePoint<T> end = getPointAt(b);
+
+  return getSegment(start, end);
+}
+
+// _____________________________________________________________________________
+template <typename T>
+PolyLine<T> PolyLine<T>::getSegmentAtDist(double a, double b) const {
+  if (a > b) {
+    double c = a;
+    a = b;
+    b = c;
+  }
+  LinePoint<T> start = getPointAtDist(a);
+  LinePoint<T> end = getPointAtDist(b);
+
+  return getSegment(start, end);
+}
+
+// _____________________________________________________________________________
+template <typename T>
+PolyLine<T> PolyLine<T>::getSegment(const Point<T>& a,
+                                    const Point<T>& b) const {
+  LinePoint<T> start = projectOn(a);
+  LinePoint<T> end = projectOnAfter(b, start.lastIndex);
+
+  return getSegment(start, end);
+}
+
+// _____________________________________________________________________________
+template <typename T>
+PolyLine<T> PolyLine<T>::getSegment(const LinePoint<T>& start,
+                                    const LinePoint<T>& end) const {
+  PolyLine ret;
+  ret << start.p;
+
+  if (start.lastIndex + 1 <= end.lastIndex) {
+    ret._line.insert(ret._line.end(), _line.begin() + start.lastIndex + 1,
+                     _line.begin() + end.lastIndex + 1);
+  }
+  ret << end.p;
+
+  // find a more performant way to clear the result of above
+  ret.simplify(0);
+
+  return ret;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+LinePoint<T> PolyLine<T>::getPointAtDist(double atDist) const {
+  if (atDist > getLength()) atDist = getLength();
+  if (atDist < 0) atDist = 0;
+
+  double dist = 0;
+
+  if (_line.size() == 1) return LinePoint<T>(0, 0, _line[0]);
+
+  const Point<T>* last = &_line[0];
+
+  for (size_t i = 1; i < _line.size(); i++) {
+    const Point<T>& cur = _line[i];
+    double d = geo::dist(last, cur);
+    dist += d;
+
+    if (dist > atDist) {
+      double p = (d - (dist - atDist));
+      return LinePoint<T>(i - 1, atDist / getLength(),
+                          interpolate(*last, cur, p));
+    }
+
+    last = &_line[i];
+  }
+
+  return LinePoint<T>(_line.size() - 1, 1, _line.back());
+}
+
+// _____________________________________________________________________________
+template <typename T>
+LinePoint<T> PolyLine<T>::getPointAt(double at) const {
+  at *= getLength();
+  return getPointAtDist(at);
+}
+
+// _____________________________________________________________________________
+template <typename T>
+Point<T> PolyLine<T>::interpolate(const Point<T>& a, const Point<T>& b,
+                                  double p) const {
+  double n1 = b.template get<0>() - a.template get<0>();
+  double n2 = b.template get<1>() - a.template get<1>();
+  double n = sqrt(n1 * n1 + n2 * n2);
+  n1 = n1 / n;
+  n2 = n2 / n;
+  return Point<T>(a.template get<0>() + (n1 * p),
+                  a.template get<1>() + (n2 * p));
+}
+
+// _____________________________________________________________________________
+template <typename T>
+double PolyLine<T>::distTo(const PolyLine<T>& g) const {
+  return dist(_line, g.getLine());
+}
+
+// _____________________________________________________________________________
+template <typename T>
+double PolyLine<T>::distTo(const Point<T>& p) const {
+  return dist(_line, p);
+}
+
+// _____________________________________________________________________________
+template <typename T>
+double PolyLine<T>::getLength() const {
+  return len(_line);
+}
+
+// _____________________________________________________________________________
+template <typename T>
+PolyLine<T> PolyLine<T>::average(const std::vector<const PolyLine<T>*>& lines,
+                                 const std::vector<double>& weights) {
+  bool weighted = lines.size() == weights.size();
+  double stepSize;
+
+  double longestLength = DBL_MIN;  // avoid recalc of length on each comparision
+  for (const PolyLine* p : lines) {
+    if (p->getLength() > longestLength) {
+      longestLength = p->getLength();
+    }
+  }
+
+  PolyLine ret;
+  double total = 0;
+
+  for (size_t i = 0; i < lines.size(); ++i) {
+    if (weighted) {
+      total += weights[i];
+    } else {
+      total += 1;
+    }
+  }
+
+  stepSize = AVERAGING_STEP / longestLength;
+  bool end = false;
+  for (double a = 0; !end; a += stepSize) {
+    if (a > 1) {
+      a = 1;
+      end = true;
+    }
+    double x = 0, y = 0;
+
+    for (size_t i = 0; i < lines.size(); ++i) {
+      const PolyLine* pl = lines[i];
+      Point<T> p = pl->getPointAt(a).p;
+      if (weighted) {
+        x += p.template get<0>() * weights[i];
+        y += p.template get<1>() * weights[i];
+      } else {
+        x += p.template get<0>();
+        y += p.template get<1>();
+      }
+    }
+    ret << Point<T>(x / total, y / total);
+  }
+
+  ret.simplify(0);
+
+  return ret;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+PolyLine<T> PolyLine<T>::average(const std::vector<const PolyLine<T>*>& lines) {
+  return average(lines, std::vector<double>());
+}
+
+// _____________________________________________________________________________
+template <typename T>
+std::pair<size_t, double> PolyLine<T>::nearestSegmentAfter(const Point<T>& p,
+                                                           size_t a) const {
+  // returns the index of the starting point of the nearest segment of p
+  assert(a < _line.size());
+
+  double totalLength = getLength();
+  size_t smallest = a;
+  double totalDist = 0;
+  double dist = DBL_MAX;
+  double smallestDist = 0;
+
+  for (size_t i = smallest + 1; i < _line.size(); i++) {
+    Point<T> startP(_line[i - 1]);
+    Point<T> endP(_line[i]);
+
+    if (i > 1) {
+      totalDist += geo::dist(_line[i - 2], _line[i - 1]);
+    }
+
+    double curDist = distToSegment(startP, endP, p);
+
+    if (curDist < dist) {
+      dist = curDist;
+      smallest = i - 1;
+      smallestDist = totalDist;
+    }
+  }
+
+  if (totalLength > 0) {
+    smallestDist /= totalLength;
+  } else {
+    smallestDist = 0;
+  }
+
+  return std::pair<size_t, double>(smallest, smallestDist);
+}
+
+// _____________________________________________________________________________
+template <typename T>
+std::pair<size_t, double> PolyLine<T>::nearestSegment(const Point<T>& p) const {
+  return nearestSegmentAfter(p, 0);
+}
+
+// _____________________________________________________________________________
+template <typename T>
+LinePoint<T> PolyLine<T>::projectOn(const Point<T>& p) const {
+  return projectOnAfter(p, 0);
+}
+
+// _____________________________________________________________________________
+template <typename T>
+LinePoint<T> PolyLine<T>::projectOnAfter(const Point<T>& p, size_t a) const {
+  assert(a < _line.size());
+  std::pair<size_t, double> bc = nearestSegmentAfter(p, a);
+
+  Point<T> ret = geo::projectOn(_line[bc.first], p, _line[bc.first + 1]);
+
+  if (getLength() > 0) {
+    bc.second += dist(_line[bc.first], ret) / getLength();
+  }
+
+  return LinePoint<T>(bc.first, bc.second, ret);
+}
+
+// _____________________________________________________________________________
+template <typename T>
+void PolyLine<T>::simplify(double d) {
+  _line = geo::simplify(_line, d);
+}
+
+// _____________________________________________________________________________
+template <typename T>
+void PolyLine<T>::smoothenOutliers(double d) {
+  if (_line.size() < 3) return;
+  for (size_t i = 1; i < _line.size() - 3; ++i) {
+    double ang = innerProd(_line[i], _line[i - 1], _line[i + 1]);
+
+    if (dist(_line[i], _line[i + 1]) < d || dist(_line[i], _line[i - 1]) < d) {
+      if (ang < 35) {
+        _line.erase(_line.begin() + i);
+      }
+    }
+  }
+}
+
+// _____________________________________________________________________________
+template <typename T>
+bool PolyLine<T>::equals(const PolyLine<T>& rhs) const {
+  // TODO: why 100? make global static or configurable or determine in some
+  //       way!
+  return equals(rhs, 100);
+}
+
+// _____________________________________________________________________________
+template <typename T>
+bool PolyLine<T>::operator==(const PolyLine<T>& rhs) const {
+  // TODO: why 100? make global static or configurable or determine in some
+  //       way!
+  return equals(rhs, 100);
+}
+
+// _____________________________________________________________________________
+template <typename T>
+bool PolyLine<T>::equals(const PolyLine<T>& rhs, double dmax) const {
+  // check if two lines are equal, THE DIRECTION DOES NOT MATTER HERE!!!!!
+
+  if (_line.size() == 2 && _line.size() == rhs.getLine().size()) {
+    // trivial case, straight line, implement directly
+    return (dist(_line[0], rhs.getLine()[0]) < dmax &&
+            dist(_line.back(), rhs.back()) < dmax) ||
+           (dist(_line[0], rhs.back()) < dmax &&
+            dist(_line.back(), rhs.getLine()[0]) < dmax);
+  } else {
+    return contains(rhs, dmax) && rhs.contains(*this, dmax);
+  }
+
+  return true;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+bool PolyLine<T>::contains(const PolyLine<T>& rhs, double dmax) const {
+  // check if two lines are equal. Line direction does not matter here.
+
+  for (size_t i = 0; i < rhs.getLine().size(); ++i) {
+    double d = dist(rhs.getLine()[i], getLine());
+    if (d > dmax) {
+      return false;
+    }
+  }
+
+  return true;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+void PolyLine<T>::move(double vx, double vy) {
+  for (size_t i = 0; i < _line.size(); i++) {
+    _line[i].set<0>(_line[i].template get<0>() + vx);
+    _line[i].set<1>(_line[i].template get<1>() + vy);
+  }
+}
+
+// _____________________________________________________________________________
+template <typename T>
+SharedSegments<T> PolyLine<T>::getSharedSegments(const PolyLine<T>& pl,
+                                                 double dmax) const {
+  /**
+   * Returns the segments this polyline share with pl
+   * atm, this is a very simple distance-based algorithm
+   */
+  double STEP_SIZE = 2;
+  double MAX_SKIPS = 4;
+  double MIN_SEG_LENGTH = 1;  // dmax / 2;  // make this configurable!
+
+  SharedSegments<T> ret;
+
+  if (distTo(pl) > dmax) return ret;
+
+  bool in = false, single = true;
+  double curDist = 0;
+  double curTotalSegDist = 0;
+  size_t skips;
+
+  LinePoint<T> curStartCand, curEndCand, curStartCandCmp, curEndCandCmp;
+
+  double comp = 0, curSegDist = 0;
+  double length = getLength(), plLength = pl.getLength();
+
+  for (size_t i = 1; i < _line.size(); ++i) {
+    const Point<T>& s = _line[i - 1];
+    const Point<T>& e = _line[i];
+
+    bool lastRound = false;
+
+    double totalDist = dist(s, e);
+    while (curSegDist <= totalDist) {
+      const Point<T>& curPointer = interpolate(s, e, curSegDist);
+
+      if (pl.distTo(curPointer) <= dmax) {
+        LinePoint<T> curCmpPointer = pl.projectOn(curPointer);
+        LinePoint<T> curBackProjectedPointer = projectOn(curCmpPointer.p);
+        skips = 0;
+
+        if (in) {
+          curEndCand = curBackProjectedPointer;
+          curEndCandCmp = curCmpPointer;
+
+          single = false;
+
+          comp = fabs(curStartCand.totalPos * length -
+                      curEndCand.totalPos * length) /
+                 fabs(curStartCandCmp.totalPos * plLength -
+                      curEndCandCmp.totalPos * plLength);
+        } else {
+          in = true;
+          curStartCand = curBackProjectedPointer;
+          curStartCandCmp = curCmpPointer;
+        }
+      } else {
+        if (in) {
+          skips++;
+          if (skips > MAX_SKIPS) {  // TODO: make configurable
+            if (comp > 0.8 && comp < 1.2 && !single &&
+                (fabs(curStartCand.totalPos * length -
+                      curEndCand.totalPos * length) > MIN_SEG_LENGTH &&
+                 fabs(curStartCandCmp.totalPos * plLength -
+                      curEndCandCmp.totalPos * plLength) > MIN_SEG_LENGTH)) {
+              ret.segments.push_back(
+                  SharedSegment<T>(std::pair<LinePoint<T>, LinePoint<T>>(
+                                       curStartCand, curStartCandCmp),
+                                   std::pair<LinePoint<T>, LinePoint<T>>(
+                                       curEndCand, curEndCandCmp)));
+
+              // TODO: only return the FIRST one, make this configuralbe
+              return ret;
+            }
+
+            in = false;
+            single = true;
+          }
+        }
+      }
+
+      if (curSegDist + STEP_SIZE > totalDist && !lastRound) {
+        lastRound = true;
+        double finalStep = totalDist - curSegDist - 0.0005;
+        curSegDist += finalStep;
+        curDist += finalStep;
+      } else {
+        curSegDist += STEP_SIZE;
+        curDist += STEP_SIZE;
+      }
+    }
+
+    curSegDist = curSegDist - totalDist;
+    curTotalSegDist += totalDist;
+  }
+
+  if (comp > 0.8 && comp < 1.2 && in && !single &&
+      (fabs(curStartCand.totalPos * length - curEndCand.totalPos * length) >
+           MIN_SEG_LENGTH &&
+       fabs(curStartCandCmp.totalPos * plLength -
+            curEndCandCmp.totalPos * plLength) > MIN_SEG_LENGTH)) {
+    ret.segments.push_back(SharedSegment<T>(
+        std::pair<LinePoint<T>, LinePoint<T>>(curStartCand, curStartCandCmp),
+        std::pair<LinePoint<T>, LinePoint<T>>(curEndCand, curEndCandCmp)));
+  }
+
+  return ret;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+std::set<LinePoint<T>, LinePointCmp<T>> PolyLine<T>::getIntersections(
+    const PolyLine<T>& g) const {
+  std::set<LinePoint<T>, LinePointCmp<T>> ret;
+
+  for (size_t i = 1; i < g.getLine().size(); ++i) {
+    // for each line segment, check if it intersects with a line segment in g
+    const std::set<LinePoint<T>, LinePointCmp<T>> a =
+        getIntersections(g, i - 1, i);
+    ret.insert(a.begin(), a.end());
+  }
+
+  return ret;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+std::set<LinePoint<T>, LinePointCmp<T>> PolyLine<T>::getIntersections(
+    const PolyLine<T>& p, size_t a, size_t b) const {
+  std::set<LinePoint<T>, LinePointCmp<T>> ret;
+
+  if (dist(p.getLine()[a], p.getLine()[b]) == 0) {
+    // we cannot intersect with a point
+    return ret;
+  }
+
+  for (size_t i = 1; i < _line.size(); ++i) {
+    if (intersects(_line[i - 1], _line[i], p.getLine()[a], p.getLine()[b])) {
+      Point<T> isect =
+          intersection(_line[i - 1], _line[i], p.getLine()[a], p.getLine()[b]);
+      ret.insert(p.projectOn(isect));
+    }
+  }
+
+  return ret;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+PolyLine<T> PolyLine<T>::getOrthoLineAtDist(double d, double length) const {
+  Point<T> avgP = getPointAtDist(d).p;
+
+  double angle = angBetween(getPointAtDist(d - 5).p, getPointAtDist(d + 5).p);
+
+  double angleX1 = avgP.template get<0>() + cos(angle + M_PI / 2) * length / 2;
+  double angleY1 = avgP.template get<1>() + sin(angle + M_PI / 2) * length / 2;
+
+  double angleX2 = avgP.template get<0>() + cos(angle + M_PI / 2) * -length / 2;
+  double angleY2 = avgP.template get<1>() + sin(angle + M_PI / 2) * -length / 2;
+
+  return PolyLine(Point<T>(angleX1, angleY1), Point<T>(angleX2, angleY2));
+}
+
+// _____________________________________________________________________________
+template <typename T>
+void PolyLine<T>::empty() {
+  _line.empty();
+}
+
+// _____________________________________________________________________________
+template <typename T>
+std::pair<double, double> PolyLine<T>::getSlopeBetween(double ad,
+                                                       double bd) const {
+  LinePoint<T> a = getPointAt(ad);
+  LinePoint<T> b = getPointAt(bd);
+
+  double d = dist(a.p, b.p);
+
+  double dx = (b.p.template get<0>() - a.p.template get<0>()) / d;
+  double dy = (b.p.template get<1>() - a.p.template get<1>()) / d;
+
+  return std::pair<double, double>(dx, dy);
+}
+
+// _____________________________________________________________________________
+template <typename T>
+std::pair<double, double> PolyLine<T>::getSlopeBetweenDists(double ad,
+                                                            double bd) const {
+  return getSlopeBetween(ad / getLength(), bd / getLength());
+}
+
+// _____________________________________________________________________________
+template <typename T>
+std::string PolyLine<T>::getWKT() const {
+  std::stringstream ss;
+  ss << std::setprecision(12) << geo::getWKT(_line);
+
+  return ss.str();
+}
+
+// _____________________________________________________________________________
+template <typename T>
+void PolyLine<T>::fixTopology(double maxl) {
+  double distA = 0;
+
+  for (size_t i = 1; i < _line.size() - 1; i++) {
+    double distB =
+        distA + dist(_line[i - 1], _line[i]) + dist(_line[i], _line[i + 1]);
+    for (size_t j = i + 2; j < _line.size(); j++) {
+      if (intersects(_line[i - 1], _line[i], _line[j - 1], _line[j])) {
+        Point<T> p =
+            intersection(_line[i - 1], _line[i], _line[j - 1], _line[j]);
+
+        double posA = dist(_line[i - 1], p) + distA;
+        double posB = dist(_line[j - 1], p) + distB;
+
+        if (fabs(posA - posB) < maxl) {
+          _line[i] = p;
+          _line.erase(_line.begin() + i + 1, _line.begin() + j);
+        }
+      }
+
+      distB += dist(_line[j - 1], _line[j]);
+    }
+    distA += dist(_line[i - 1], _line[i]);
+  }
+}
+
+// _____________________________________________________________________________
+template <typename T>
+void PolyLine<T>::applyChaikinSmooth(size_t depth) {
+  for (size_t i = 0; i < depth; i++) {
+    Line<T> smooth;
+
+    smooth.push_back(_line.front());
+
+    for (size_t i = 1; i < _line.size(); i++) {
+      Point<T> pA = _line[i - 1];
+      Point<T> pB = _line[i];
+
+      smooth.push_back(
+          Point<T>(0.75 * pA.template get<0>() + 0.25 * pB.template get<0>(),
+                   0.75 * pA.template get<1>() + 0.25 * pB.template get<1>()));
+      smooth.push_back(
+          Point<T>(0.25 * pA.template get<0>() + 0.75 * pB.template get<0>(),
+                   0.25 * pA.template get<1>() + 0.75 * pB.template get<1>()));
+    }
+
+    smooth.push_back(_line.back());
+    _line = smooth;
+  }
+}
+
+// _____________________________________________________________________________
+template <typename T>
+const Point<T>& PolyLine<T>::front() const {
+  return _line.front();
+}
+
+// _____________________________________________________________________________
+template <typename T>
+const Point<T>& PolyLine<T>::back() const {
+  return _line.back();
+}

src/util/geo/output/GeoGraphJsonOutput.h

@@ -0,0 +1,38 @@
+// Copyright 2016, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+#ifndef UTIL_GEO_OUTPUT_GEOGRAPHJSONOUTPUT_H_
+#define UTIL_GEO_OUTPUT_GEOGRAPHJSONOUTPUT_H_
+
+#include <ostream>
+#include <string>
+#include "util/String.h"
+#include "util/geo/output/GeoJsonOutput.h"
+#include "util/graph/Graph.h"
+
+using util::toString;
+using util::graph::Graph;
+
+namespace util {
+namespace geo {
+namespace output {
+
+class GeoGraphJsonOutput {
+ public:
+  inline GeoGraphJsonOutput(){};
+  template <typename N, typename E>
+  void print(const Graph<N, E>& outG, std::ostream& str);
+
+ private:
+  template <typename T>
+  Line<T> createLine(const util::geo::Point<T>& a,
+                     const util::geo::Point<T>& b);
+};
+
+#include "util/geo/output/GeoGraphJsonOutput.tpp"
+}
+}
+}
+
+#endif  // UTIL_GEO_OUTPUT_GEOGRAPHJSONOUTPUT_H_

src/util/geo/output/GeoGraphJsonOutput.tpp

@@ -0,0 +1,62 @@
+// Copyright 2016, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+using util::geo::output::Attrs;
+
+// _____________________________________________________________________________
+template <typename T>
+Line<T> GeoGraphJsonOutput::createLine(const util::geo::Point<T>& a,
+                                       const util::geo::Point<T>& b) {
+  Line<T> ret;
+  ret.push_back(a);
+  ret.push_back(b);
+  return ret;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+void GeoGraphJsonOutput::print(const util::graph::Graph<N, E>& outG,
+                               std::ostream& str) {
+  GeoJsonOutput _out(str);
+
+  // first pass, nodes
+  for (util::graph::Node<N, E>* n : outG.getNds()) {
+    if (!n->pl().getGeom()) continue;
+
+    Attrs props = {{"id", toString(n)},
+                   {"deg", toString(n->getInDeg() + n->getOutDeg())},
+                   {"deg_out", toString(n->getOutDeg())},
+                   {"deg_in", toString(n->getInDeg())}};
+    n->pl().getAttrs(&props);
+
+    _out.print(*n->pl().getGeom(), props);
+  }
+
+  // second pass, edges
+  for (graph::Node<N, E>* n : outG.getNds()) {
+    for (graph::Edge<N, E>* e : n->getAdjListOut()) {
+      // to avoid double output for undirected graphs
+      if (e->getFrom() != n) continue;
+      Attrs props{{"from", toString(e->getFrom())},
+                  {"to", toString(e->getTo())},
+                  {"id", toString(e)}};
+
+      e->pl().getAttrs(&props);
+
+      if (!e->pl().getGeom() || !e->pl().getGeom()->size()) {
+        if (e->getFrom()->pl().getGeom()) {
+          auto a = *e->getFrom()->pl().getGeom();
+          if (e->getTo()->pl().getGeom()) {
+            auto b = *e->getTo()->pl().getGeom();
+            _out.print(createLine(a, b), props);
+          }
+        }
+      } else {
+        _out.print(*e->pl().getGeom(), props);
+      }
+    }
+  }
+
+  _out.flush();
+}

src/util/geo/output/GeoJsonOutput.cpp

@@ -0,0 +1,25 @@
+// Copyright 2016, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+//
+#include "util/geo/output/GeoJsonOutput.h"
+
+using namespace util;
+using namespace geo;
+using namespace output;
+
+// _____________________________________________________________________________
+GeoJsonOutput::GeoJsonOutput(std::ostream& str) : _wr(&str, 10, true) {
+  _wr.obj();
+  _wr.keyVal("type", "FeatureCollection");
+  _wr.key("features");
+  _wr.arr();
+}
+
+// _____________________________________________________________________________
+GeoJsonOutput::~GeoJsonOutput() {
+  flush();
+}
+
+// _____________________________________________________________________________
+void GeoJsonOutput::flush() { _wr.closeAll(); }

src/util/geo/output/GeoJsonOutput.h

@@ -0,0 +1,41 @@
+// Copyright 2016, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+#ifndef UTIL_GEO_OUTPUT_GEOJSONOUTPUT_H_
+#define UTIL_GEO_OUTPUT_GEOJSONOUTPUT_H_
+
+#include <ostream>
+#include <string>
+#include <map>
+#include "util/String.h"
+#include "util/geo/Geo.h"
+#include "util/json/JsonWriter.h"
+
+namespace util {
+namespace geo {
+namespace output {
+
+typedef std::map<std::string, std::string> Attrs;
+
+class GeoJsonOutput {
+ public:
+  GeoJsonOutput(std::ostream& str);
+  ~GeoJsonOutput();
+  template <typename T>
+  void print(const Point<T>& p, Attrs attrs);
+  template <typename T>
+  void print(const Line<T>& l, Attrs attrs);
+  void flush();
+
+ private:
+  json::JsonWriter _wr;
+};
+
+#include "util/geo/output/GeoJsonOutput.tpp"
+
+}
+}
+}
+
+#endif  // UTIL_GEO_OUTPUT_GEOJSONOUTPUT_H_

src/util/geo/output/GeoJsonOutput.tpp

@@ -0,0 +1,48 @@
+// Copyright 2016, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+// _____________________________________________________________________________
+template <typename T>
+void GeoJsonOutput::print(const Point<T>& p, Attrs attrs) {
+  _wr.obj();
+  _wr.keyVal("type", "Feature");
+
+  _wr.key("geometry");
+  _wr.obj();
+  _wr.keyVal("type", "Point");
+  _wr.key("coordinates");
+  _wr.arr();
+  _wr.val(p.template get<0>());
+  _wr.val(p.template get<1>());
+  _wr.close();
+  _wr.close();
+  _wr.key("properties");
+  _wr.obj(attrs);
+  _wr.close();
+}
+
+// _____________________________________________________________________________
+template <typename T>
+void GeoJsonOutput::print(const Line<T>& line, Attrs attrs) {
+  if (!line.size()) return;
+  _wr.obj();
+  _wr.keyVal("type", "Feature");
+
+  _wr.key("geometry");
+  _wr.obj();
+  _wr.keyVal("type", "LineString");
+  _wr.key("coordinates");
+  _wr.arr();
+  for (auto p : line) {
+    _wr.arr();
+    _wr.val(p.template get<0>());
+    _wr.val(p.template get<1>());
+    _wr.close();
+  }
+  _wr.close();
+  _wr.close();
+  _wr.key("properties");
+  _wr.obj(attrs);
+  _wr.close();
+}