initial commit

src/util/CMakeLists.txt

@@ -0,0 +1,5 @@
+file(GLOB_RECURSE util_SRC *.cpp)
+list(REMOVE_ITEM util_SRC TestMain.cpp)
+add_library(util ${util_SRC})
+
+add_subdirectory(tests)

src/util/Misc.h

@@ -0,0 +1,78 @@
+// Copyright 2017, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+#ifndef UTIL_MISC_H_
+#define UTIL_MISC_H_
+
+#include <chrono>
+
+#define UNUSED(expr) do { (void)(expr); } while (0)
+#define TIME() std::chrono::high_resolution_clock::now()
+#define TOOK(t1, t2) (std::chrono::duration_cast<microseconds>(t2 - t1).count() / 1000.0)
+
+namespace util {
+
+// cached first 10 powers of 10
+static int pow10[10] = {
+    1,           10,          100,           1000,          10000,
+    100000,      1000000,     10000000,      100000000,     1000000000};
+
+// _____________________________________________________________________________
+inline uint64_t factorial(uint64_t n) {
+  if (n == 1) return n;
+  return n * factorial(n - 1);
+}
+
+// _____________________________________________________________________________
+inline uint64_t atoul(const char* p) {
+  uint64_t ret = 0;
+
+  while (*p) {
+    ret = ret * 10 + (*p++ - '0');
+  }
+
+  return ret;
+}
+
+// _____________________________________________________________________________
+inline float atof(const char* p, uint8_t mn) {
+  // this atof implementation works only on "normal" float strings like
+  // 56.445 or -345.00, but should be faster than std::atof
+  float ret = 0.0;
+  bool neg = false;
+  if (*p == '-') {
+    neg = true;
+    p++;
+  }
+
+  while (*p >= '0' && *p <= '9') {
+    ret = ret * 10.0 + (*p - '0');
+    p++;
+  }
+
+  if (*p == '.') {
+    p++;
+    float f = 0;
+    uint8_t n = 0;
+
+    for (; n < mn && *p >= '0' && *p <= '9'; n++, p++) {
+      f = f * 10.0 + (*p - '0');
+    }
+
+    if (n < 11)
+      ret += f / pow10[n];
+    else
+      ret += f / std::pow(10, n);
+  }
+
+  if (neg) return -ret;
+  return ret;
+}
+
+// _____________________________________________________________________________
+inline double atof(const char* p) { return atof(p, 38); }
+
+}  // namespace util
+
+#endif  // UTIL_MISC_H_

src/util/Nullable.h

@@ -0,0 +1,114 @@
+// Copyright 2017, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+#ifndef UTIL_NULLABLE_H_
+#define UTIL_NULLABLE_H_
+
+namespace util {
+
+template<typename T>
+class Nullable {
+ public:
+  Nullable()
+   : val(), null(true) {}
+  Nullable(T* valPointer)
+   : val(), null(true) {
+   if (valPointer) {
+     assign(*valPointer);
+   }
+  }
+  Nullable(const T& value)
+   : val(value), null(false) {}
+  Nullable(const Nullable& other)
+   : val(other.val), null(other.isNull()) {}
+
+  Nullable& operator=(const Nullable& other) {
+    val = other.get();
+    null = other.isNull();
+    return *this;
+  }
+
+  T operator=(const T& other) {
+    assign(other);
+    return val;
+  }
+
+  /**
+   * Passing through comparision operators
+   */
+
+  bool operator==(const Nullable& other) const {
+    return (other.isNull() && isNull()) || other.get() == get();
+  }
+
+  bool operator!=(const Nullable& other) const {
+    return !(*this == other);
+  }
+
+  bool operator<(const Nullable& other) const {
+    return !other.isNull() && !isNull() && get() < other.get();
+  }
+
+  bool operator>(const Nullable& other) const {
+    return !(*this < other || *this == other);
+  }
+
+  bool operator<=(const Nullable& other) const {
+    return *this < other || *this == other;
+  }
+
+  bool operator>=(const Nullable& other) const {
+    return *this > other || *this == other;
+  }
+
+  bool operator==(const T& other) const {
+    return !isNull() && other == get();
+  }
+
+  bool operator!=(const T& other) const {
+    return !(*this == other);
+  }
+
+  bool operator<(const T& other) const {
+    return !isNull() && get() < other;
+  }
+
+  bool operator>(const T& other) const {
+    return !(*this < other || *this == other);
+  }
+
+  bool operator<=(const T& other) const {
+    return *this < other || *this == other;
+  }
+
+  bool operator>=(const T& other) const {
+    return *this > other || *this == other;
+  }
+
+  operator T() const {
+    return get();
+  }
+
+  bool isNull() const {
+    return null;
+  }
+
+  T get() const {
+    if (!isNull()) return val;
+    else throw std::runtime_error("Trying to retrieve value of NULL object.");
+  }
+
+private:
+  void assign(T v) {
+    val = v;
+    null = false;
+  }
+
+  T val;
+  bool null;
+};
+
+}
+
+#endif  // UTIL_NULLABLE_H_

src/util/String.h

@@ -0,0 +1,158 @@
+// Copyright 2017, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+#ifndef UTIL_STRING_H_
+#define UTIL_STRING_H_
+
+#include <algorithm>
+#include <cstring>
+#include <sstream>
+#include <string>
+#include <vector>
+
+namespace util {
+
+// _____________________________________________________________________________
+inline std::string urlDecode(const std::string& encoded) {
+  std::string decoded;
+  for (size_t i = 0; i < encoded.size(); ++i) {
+    char c = encoded[i];
+    if (c == '%') {
+      std::string ah = encoded.substr(i + 1, 2);
+      char* nonProced = 0;
+      char hexVal = strtol(ah.c_str(), &nonProced, 16);
+
+      if (ah.find_first_of("+-") > 1 && ah.size() - strlen(nonProced) == 2) {
+        c = hexVal;
+        i += 2;
+      }
+    } else if (c == '+') {
+      c = ' ';
+    }
+    decoded += c;
+  }
+  return decoded;
+}
+
+// _____________________________________________________________________________
+inline std::string jsonStringEscape(const std::string& unescaped) {
+  std::string escaped;
+  for (size_t i = 0; i < unescaped.size(); ++i) {
+    if (unescaped[i] == '"' || unescaped[i] == '\\') {
+      escaped += "\\";
+    }
+    if (iscntrl(unescaped[i])) {
+      escaped += " ";
+    }
+    escaped += unescaped[i];
+  }
+  return escaped;
+}
+
+// _____________________________________________________________________________
+inline bool replace(std::string& subj, const std::string& from,
+                    const std::string& to) {
+  if (from.empty()) return false;
+  size_t start_pos = subj.find(from);
+  if (start_pos != std::string::npos) {
+    subj.replace(start_pos, from.length(), to);
+    return true;
+  }
+
+  return false;
+}
+
+// _____________________________________________________________________________
+inline bool replaceAll(std::string& subj, const std::string& from,
+                       const std::string& to) {
+  if (from.empty()) return false;
+  bool found = false;
+  size_t s = subj.find(from, 0);
+  for (; s != std::string::npos; s = subj.find(from, s + to.length())) {
+    found = true;
+    subj.replace(s, from.length(), to);
+  }
+
+  return found;
+}
+
+// _____________________________________________________________________________
+inline std::string unixBasename(const std::string& pathname) {
+  return {std::find_if(pathname.rbegin(), pathname.rend(),
+                       [](char c) { return c == '/'; })
+              .base(),
+          pathname.end()};
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline std::string toString(T obj) {
+  std::stringstream ss;
+  ss << obj;
+  return ss.str();
+}
+
+// _____________________________________________________________________________
+inline std::vector<std::string> split(std::string in, char sep) {
+  std::stringstream ss(in);
+  std::vector<std::string> ret(1);
+  while (std::getline(ss, ret.back(), sep)) {
+    ret.push_back("");
+  }
+  ret.pop_back();
+  return ret;
+}
+
+// _____________________________________________________________________________
+inline std::string ltrim(std::string str) {
+  str.erase(0, str.find_first_not_of(" \t\n\v\f\r"));
+  return str;
+}
+
+// _____________________________________________________________________________
+inline std::string rtrim(std::string str) {
+  str.erase(str.find_last_not_of(" \t\n\v\f\r") + 1);
+  return str;
+}
+
+// _____________________________________________________________________________
+inline std::string trim(std::string str) { return ltrim(rtrim(str)); }
+
+// _____________________________________________________________________________
+inline size_t editDist(const std::string& s1, const std::string& s2) {
+  // https://en.wikibooks.org/wiki/Algorithm_Implementation/Strings/Levenshtein_distance#C++
+  size_t len1 = s1.size();
+  size_t len2 = s2.size();
+  std::vector<size_t> cur(len2 + 1);
+  std::vector<size_t> prev(len2 + 1);
+
+  for (size_t i = 0; i < prev.size(); i++) prev[i] = i;
+
+  for (size_t i = 0; i < len1; i++) {
+    cur[0] = i + 1;
+    for (size_t j = 0; j < len2; j++) {
+      cur[j + 1] =
+          std::min(prev[1 + j] + 1,
+                   std::min(cur[j] + 1, prev[j] + (s1[i] == s2[j] ? 0 : 1)));
+    }
+    std::swap(cur, prev);
+  }
+
+  return prev[len2];
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline std::string implode(const std::vector<T>& vec, const char* del) {
+  std::stringstream ss;
+  for (size_t i = 0; i < vec.size(); i++) {
+    if (i != 0) ss << del;
+    ss << vec[i];
+  }
+
+  return ss.str();
+}
+}
+
+#endif  // UTIL_STRING_H_

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();
+}

src/util/graph/Dijkstra.cpp

@@ -0,0 +1,7 @@
+// Copyright 2017, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+#include "util/graph/Dijkstra.h"
+
+size_t util::graph::Dijkstra::ITERS = 0;

src/util/graph/Dijkstra.h

@@ -0,0 +1,113 @@
+// Copyright 2017, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+#ifndef UTIL_GRAPH_DIJKSTRA_H_
+#define UTIL_GRAPH_DIJKSTRA_H_
+
+#include <limits>
+#include <list>
+#include <queue>
+#include <set>
+#include <set>
+#include <unordered_map>
+#include "util/graph/Edge.h"
+#include "util/graph/Graph.h"
+#include "util/graph/Node.h"
+#include "util/graph/ShortestPath.h"
+
+namespace util {
+namespace graph {
+
+using util::graph::Graph;
+using util::graph::Node;
+using util::graph::Edge;
+
+// dijkstras algorithm for util graph
+class Dijkstra : public ShortestPath<Dijkstra> {
+ public:
+  template <typename N, typename E, typename C>
+  struct RouteNode {
+    RouteNode() : n(0), parent(0), d(), h(), e(0) {}
+    RouteNode(Node<N, E>* n) : n(n), parent(0), d(), h(), e(0) {}
+    RouteNode(Node<N, E>* n, Node<N, E>* parent, C d, Edge<N, E>* e)
+        : n(n), parent(parent), d(d), h(), e(e) {}
+    RouteNode(Node<N, E>* n, Node<N, E>* parent, C d, C h, Edge<N, E>* e)
+        : n(n), parent(parent), d(d), h(h), e(e) {}
+
+    Node<N, E>* n;
+    Node<N, E>* parent;
+
+    C d;
+    C h;
+
+    Edge<N, E>* e;
+
+    bool operator<(const RouteNode<N, E, C>& p) const {
+      return h > p.h || (h == p.h && d > p.d);
+    }
+  };
+
+  template <typename N, typename E, typename C>
+  using Settled = std::unordered_map<Node<N, E>*, RouteNode<N, E, C> >;
+
+  template <typename N, typename E, typename C>
+  using PQ = std::priority_queue<RouteNode<N, E, C> >;
+
+  template <typename N, typename E, typename C>
+  struct CostFunc : public ShortestPath::CostFunc<N, E, C> {
+    C operator()(const Edge<N, E>* from, const Node<N, E>* n,
+                 const Edge<N, E>* to) const {
+      UNUSED(from);
+      UNUSED(n);
+      UNUSED(to);
+      return C();
+    };
+  };
+
+  template <typename N, typename E, typename C>
+  struct HeurFunc : public ShortestPath::HeurFunc<N, E, C> {
+    C operator()(const Edge<N, E>* from,
+                 const std::set<Edge<N, E>*>& to) const {
+      UNUSED(from);
+      UNUSED(to);
+      return C();
+    };
+  };
+
+  template <typename N, typename E, typename C>
+  static std::unordered_map<Node<N, E>*, C> shortestPathImpl(
+      Node<N, E>* from, const std::set<Node<N, E>*>& to,
+      const ShortestPath::CostFunc<N, E, C>& costFunc, const ShortestPath::HeurFunc<N, E, C>&,
+      std::unordered_map<Node<N, E>*, EList<N, E>*> resEdges,
+      std::unordered_map<Node<N, E>*, NList<N, E>*> resNode);
+
+  template <typename N, typename E, typename C>
+  static C shortestPathImpl(const std::set<Node<N, E>*> from, const std::set<Node<N, E>*>& to,
+                            const ShortestPath::CostFunc<N, E, C>& costFunc,
+                            const ShortestPath::HeurFunc<N, E, C>& heurFunc,
+                            EList<N, E>* resEdges, NList<N, E>* resNodes);
+
+  template <typename N, typename E, typename C>
+  static C shortestPathImpl(Node<N, E>* from, const std::set<Node<N, E>*>& to,
+                            const ShortestPath::CostFunc<N, E, C>& costFunc,
+                            const ShortestPath::HeurFunc<N, E, C>& heurFunc,
+                            EList<N, E>* resEdges, NList<N, E>* resNodes);
+
+  template <typename N, typename E, typename C>
+  static void relax(RouteNode<N, E, C>& cur, const std::set<Node<N, E>*>& to,
+                    const ShortestPath::CostFunc<N, E, C>& costFunc,
+                    const ShortestPath::HeurFunc<N, E, C>& heurFunc, PQ<N, E, C>& pq);
+
+  template <typename N, typename E, typename C>
+  static void buildPath(Node<N, E>* curN, Settled<N, E, C>& settled,
+                        NList<N, E>* resNodes, EList<N, E>* resEdges);
+
+  static size_t ITERS;
+};
+
+#include "util/graph/Dijkstra.tpp"
+}
+}
+
+#endif  // UTIL_GRAPH_DIJKSTRA_H_

src/util/graph/Dijkstra.tpp

@@ -0,0 +1,175 @@
+// Copyright 2017, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+// _____________________________________________________________________________
+template <typename N, typename E, typename C>
+C Dijkstra::shortestPathImpl(Node<N, E>* from, const std::set<Node<N, E>*>& to,
+                             const ShortestPath::CostFunc<N, E, C>& costFunc,
+                             const ShortestPath::HeurFunc<N, E, C>& heurFunc,
+                             EList<N, E>* resEdges, NList<N, E>* resNodes) {
+  if (from->getOutDeg() == 0) return costFunc.inf();
+
+  Settled<N, E, C> settled;
+  PQ<N, E, C> pq;
+  bool found = false;
+
+  pq.emplace(from);
+  RouteNode<N, E, C> cur;
+
+  while (!pq.empty()) {
+    Dijkstra::ITERS++;
+
+    if (settled.find(pq.top().n) != settled.end()) {
+      pq.pop();
+      continue;
+    }
+
+    cur = pq.top();
+    pq.pop();
+
+    settled[cur.n] = cur;
+
+    if (to.find(cur.n) != to.end()) {
+      found = true;
+      break;
+    }
+
+    relax(cur, to, costFunc, heurFunc, pq);
+  }
+
+  if (!found) return costFunc.inf();
+
+  buildPath(cur.n, settled, resNodes, resEdges);
+
+  return cur.d;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E, typename C>
+C Dijkstra::shortestPathImpl(const std::set<Node<N, E>*> from,
+                             const std::set<Node<N, E>*>& to,
+                             const ShortestPath::CostFunc<N, E, C>& costFunc,
+                             const ShortestPath::HeurFunc<N, E, C>& heurFunc,
+                             EList<N, E>* resEdges, NList<N, E>* resNodes) {
+  Settled<N, E, C> settled;
+  PQ<N, E, C> pq;
+  bool found = false;
+
+  // put all nodes in from onto PQ
+  for (auto n : from) pq.emplace(n);
+  RouteNode<N, E, C> cur;
+
+  while (!pq.empty()) {
+    Dijkstra::ITERS++;
+
+    if (settled.find(pq.top().n) != settled.end()) {
+      pq.pop();
+      continue;
+    }
+
+    cur = pq.top();
+    pq.pop();
+
+    settled[cur.n] = cur;
+
+    if (to.find(cur.n) != to.end()) {
+      found = true;
+      break;
+    }
+
+    relax(cur, to, costFunc, heurFunc, pq);
+  }
+
+  if (!found) return costFunc.inf();
+
+  buildPath(cur.n, settled, resNodes, resEdges);
+
+  return cur.d;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E, typename C>
+std::unordered_map<Node<N, E>*, C> Dijkstra::shortestPathImpl(
+    Node<N, E>* from, const std::set<Node<N, E>*>& to,
+    const ShortestPath::CostFunc<N, E, C>& costFunc,
+    const ShortestPath::HeurFunc<N, E, C>& heurFunc,
+    std::unordered_map<Node<N, E>*, EList<N, E>*> resEdges,
+    std::unordered_map<Node<N, E>*, NList<N, E>*> resNodes) {
+  std::unordered_map<Node<N, E>*, C> costs;
+  if (to.size() == 0) return costs;
+  // init costs with inf
+  for (auto n : to) costs[n] = costFunc.inf();
+
+  if (from->getOutDeg() == 0) return costs;
+
+  Settled<N, E, C> settled;
+  PQ<N, E, C> pq;
+
+  size_t found = 0;
+
+  pq.emplace(from);
+  RouteNode<N, E, C> cur;
+
+  while (!pq.empty()) {
+    Dijkstra::ITERS++;
+
+    if (settled.find(pq.top().n) != settled.end()) {
+      pq.pop();
+      continue;
+    }
+
+    cur = pq.top();
+    pq.pop();
+
+    settled[cur.n] = cur;
+
+    if (to.find(cur.n) != to.end()) {
+      found++;
+    }
+
+    if (found == to.size()) break;
+
+    relax(cur, to, costFunc, heurFunc, pq);
+  }
+
+  for (auto nto : to) {
+    if (!settled.count(nto)) continue;
+    Node<N, E>* curN = nto;
+    costs[nto] = settled[curN].d;
+
+    buildPath(nto, settled, resNodes[nto], resEdges[nto]);
+  }
+
+  return costs;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E, typename C>
+void Dijkstra::relax(RouteNode<N, E, C>& cur, const std::set<Node<N, E>*>& to,
+                     const ShortestPath::CostFunc<N, E, C>& costFunc,
+                     const ShortestPath::HeurFunc<N, E, C>& heurFunc, PQ<N, E, C>& pq) {
+  for (auto edge : cur.n->getAdjListOut()) {
+    C newC = costFunc(cur.n, edge, edge->getOtherNd(cur.n));
+    newC = cur.d + newC;
+    if (costFunc.inf() <= newC) continue;
+
+    const C& newH = newC + heurFunc(edge->getOtherNd(cur.n), to);
+
+    pq.emplace(edge->getOtherNd(cur.n), cur.n, newC, newH, &(*edge));
+  }
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E, typename C>
+void Dijkstra::buildPath(Node<N, E>* curN,
+                         Settled<N, E, C>& settled, NList<N, E>* resNodes,
+                         EList<N, E>* resEdges) {
+  while (true) {
+    const RouteNode<N, E, C>& curNode = settled[curN];
+    if (resNodes) resNodes->push_back(curNode.n);
+    if (!curNode.parent) break;
+    if (resEdges) resEdges->push_back(curNode.e);
+    curN = curNode.parent;
+  }
+}

src/util/graph/DirGraph.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_GRAPH_DIRGRAPH_H_
+#define UTIL_GRAPH_DIRGRAPH_H_
+
+#include <set>
+#include <string>
+
+#include "util/graph/Graph.h"
+#include "util/graph/Edge.h"
+#include "util/graph/DirNode.h"
+
+namespace util {
+namespace graph {
+
+template <typename N, typename E>
+using UndirEdge = Edge<N, E>;
+
+template <typename N, typename E>
+class DirGraph : public Graph<N, E> {
+ public:
+  explicit DirGraph();
+
+  using Graph<N, E>::addEdg;
+
+  Node<N, E>* addNd();
+  Node<N, E>* addNd(DirNode<N, E>* n);
+  Node<N, E>* addNd(const N& pl);
+  Edge<N, E>* addEdg(Node<N, E>* from, Node<N, E>* to, const E& p);
+
+  Node<N, E>* mergeNds(Node<N, E>* a, Node<N, E>* b);
+
+};
+
+#include "util/graph/DirGraph.tpp"
+}
+}
+
+#endif  // UTIL_GRAPH_DIRGRAPH_H_

src/util/graph/DirGraph.tpp

@@ -0,0 +1,59 @@
+// Copyright 2016, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+DirGraph<N, E>::DirGraph() {}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+Node<N, E>* DirGraph<N, E>::addNd(const N& pl) {
+  return addNd(new DirNode<N, E>(pl));
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+Node<N, E>* DirGraph<N, E>::addNd() {
+  return addNd(new DirNode<N, E>());
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+Node<N, E>* DirGraph<N, E>::addNd(DirNode<N, E>* n) {
+  auto ins = Graph<N, E>::getNds()->insert(n);
+  return *ins.first;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+Edge<N, E>* DirGraph<N, E>::addEdg(Node<N, E>* from, Node<N, E>* to,
+                                    const E& p) {
+  Edge<N, E>* e = Graph<N, E>::getEdg(from, to);
+  if (!e) {
+    e = new Edge<N, E>(from, to, p);
+    from->addEdge(e);
+    to->addEdge(e);
+  }
+  return e;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+Node<N, E>* DirGraph<N, E>::mergeNds(Node<N, E>* a, Node<N, E>* b) {
+  for (auto e : a->getAdjListOut()) {
+    if (e->getTo() != b) {
+      addEdg(b, e->getTo(), e->pl());
+    }
+  }
+
+  for (auto e : a->getAdjListIn()) {
+    if (e->getFrom() != b) {
+      addEdg(e->getFrom(), b, e->pl());
+    }
+  }
+
+  DirGraph<N, E>::delNd(a);
+
+  return b;
+}

src/util/graph/DirNode.h

@@ -0,0 +1,57 @@
+// Copyright 2016, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+#ifndef UTIL_GRAPH_DIRNODE_H_
+#define UTIL_GRAPH_DIRNODE_H_
+
+#include <vector>
+#include "util/graph/Node.h"
+
+namespace util {
+namespace graph {
+
+// forward declaration of Edge
+
+template <typename N, typename E>
+class DirNode : public Node<N, E> {
+ public:
+  DirNode();
+  DirNode(const N& pl);
+  ~DirNode();
+
+  const std::vector<Edge<N, E>*>& getAdjList() const;
+  const std::vector<Edge<N, E>*>& getAdjListIn() const;
+  const std::vector<Edge<N, E>*>& getAdjListOut() const;
+
+  size_t getDeg() const;
+  size_t getInDeg() const;
+  size_t getOutDeg() const;
+
+  bool hasEdgeIn(const Edge<N, E>* e) const;
+  bool hasEdgeOut(const Edge<N, E>* e) const;
+  bool hasEdge(const Edge<N, E>* e) const;
+
+  // add edge to this node's adjacency lists
+  void addEdge(Edge<N, E>* e);
+
+  // remove edge from this node's adjacency lists
+  void removeEdge(Edge<N, E>* e);
+
+  N& pl();
+  const N& pl() const;
+
+ private:
+  std::vector<Edge<N, E>*> _adjListIn;
+  std::vector<Edge<N, E>*> _adjListOut;
+  N _pl;
+
+  bool adjInContains(const Edge<N, E>* e) const;
+  bool adjOutContains(const Edge<N, E>* e) const;
+};
+
+#include "util/graph/DirNode.tpp"
+
+}}
+
+#endif  // UTIL_GRAPH_DIRNODE_H_

src/util/graph/DirNode.tpp

@@ -0,0 +1,153 @@
+// Copyright 2016, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+DirNode<N, E>::DirNode() : _pl() {}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+DirNode<N, E>::DirNode(const N& pl) : _pl(pl) {}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+DirNode<N, E>::~DirNode() {
+  // delete self edges
+  for (auto e = _adjListOut.begin(); e != _adjListOut.end();) {
+    Edge<N, E>* eP = *e;
+    if (eP->getTo() == this) {
+      _adjListIn.erase(std::find(_adjListIn.begin(), _adjListIn.end(), eP));
+      e = _adjListOut.erase(e);
+      delete eP;
+    } else {
+      e++;
+    }
+  }
+
+  for (auto e = _adjListOut.begin(); e != _adjListOut.end(); e++) {
+    Edge<N, E>* eP = *e;
+
+    if (eP->getTo() != this) {
+      eP->getTo()->removeEdge(eP);
+      delete eP;
+    }
+  }
+
+  for (auto e = _adjListIn.begin(); e != _adjListIn.end(); e++) {
+    Edge<N, E>* eP = *e;
+
+    if (eP->getFrom() != this) {
+      eP->getFrom()->removeEdge(eP);
+      delete eP;
+    }
+  }
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+void DirNode<N, E>::addEdge(Edge<N, E>* e) {
+  if (e->getFrom() == this && !adjOutContains(e)) {
+    _adjListOut.reserve(_adjListOut.size() + 1);
+    _adjListOut.push_back(e);
+  }
+  if (e->getTo() == this && !adjInContains(e)) {
+    _adjListIn.reserve(_adjListIn.size() + 1);
+    _adjListIn.push_back(e);
+  }
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+void DirNode<N, E>::removeEdge(Edge<N, E>* e) {
+  if (e->getFrom() == this) {
+    auto p = std::find(_adjListOut.begin(), _adjListOut.end(), e);
+    if (p != _adjListOut.end()) _adjListOut.erase(p);
+  }
+  if (e->getTo() == this) {
+    auto p = std::find(_adjListIn.begin(), _adjListIn.end(), e);
+    if (p != _adjListIn.end()) _adjListIn.erase(p);
+  }
+}
+//
+// _____________________________________________________________________________
+template <typename N, typename E>
+bool DirNode<N, E>::hasEdgeIn(const Edge<N, E>* e) const {
+  return e->getTo() == this;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+bool DirNode<N, E>::hasEdgeOut(const Edge<N, E>* e) const {
+  return e->getFrom() == this;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+bool DirNode<N, E>::hasEdge(const Edge<N, E>* e) const {
+  return hasEdgeOut(e) || hasEdgeIn(e);
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+const std::vector<Edge<N, E>*>& DirNode<N, E>::getAdjList() const {
+  return _adjListOut;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+const std::vector<Edge<N, E>*>& DirNode<N, E>::getAdjListOut() const {
+  return _adjListOut;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+const std::vector<Edge<N, E>*>& DirNode<N, E>::getAdjListIn() const {
+  return _adjListIn;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+size_t DirNode<N, E>::getDeg() const {
+  return _adjListOut.size();
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+size_t DirNode<N, E>::getInDeg() const {
+  return _adjListIn.size();
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+size_t DirNode<N, E>::getOutDeg() const {
+  return _adjListOut.size();
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+N& DirNode<N, E>::pl() {
+  return _pl;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+const N& DirNode<N, E>::pl() const {
+  return _pl;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+bool DirNode<N, E>::adjInContains(const Edge<N, E>* e) const {
+  for (size_t i = 0; i < _adjListIn.size(); i++)
+    if (_adjListIn[i] == e) return true;
+  return false;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+bool DirNode<N, E>::adjOutContains(const Edge<N, E>* e) const {
+  for (size_t i = 0; i < _adjListOut.size(); i++)
+    if (_adjListOut[i] == e) return true;
+  return false;
+}

src/util/graph/EDijkstra.cpp

@@ -0,0 +1,7 @@
+// Copyright 2017, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+#include "util/graph/EDijkstra.h"
+
+size_t util::graph::EDijkstra::ITERS = 0;

src/util/graph/EDijkstra.h

@@ -0,0 +1,139 @@
+// Copyright 2017, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+#ifndef UTIL_GRAPH_EDIJKSTRA_H_
+#define UTIL_GRAPH_EDIJKSTRA_H_
+
+#include <limits>
+#include <list>
+#include <queue>
+#include <set>
+#include <unordered_map>
+#include "util/graph/Edge.h"
+#include "util/graph/Graph.h"
+#include "util/graph/Node.h"
+#include "util/graph/ShortestPath.h"
+
+namespace util {
+namespace graph {
+
+using util::graph::Graph;
+using util::graph::Node;
+using util::graph::Edge;
+
+// edge-based dijkstra - settles edges instead of nodes
+class EDijkstra : public ShortestPath<EDijkstra> {
+ public:
+  template <typename N, typename E, typename C>
+  struct RouteEdge {
+    RouteEdge() : e(0), parent(0), d(), h(), n(0) {}
+    RouteEdge(Edge<N, E>* e) : e(e), parent(0), d(), h(), n(0) {}
+    RouteEdge(Edge<N, E>* e, Edge<N, E>* parent, Node<N, E>* n, C d)
+        : e(e), parent(parent), d(d), h(), n(n) {}
+    RouteEdge(Edge<N, E>* e, Edge<N, E>* parent, Node<N, E>* n, C d, C h)
+        : e(e), parent(parent), d(d), h(h), n(n) {}
+
+    Edge<N, E>* e;
+    Edge<N, E>* parent;
+
+    C d;
+    C h;
+
+    Node<N, E>* n;
+
+    bool operator<(const RouteEdge<N, E, C>& p) const {
+      return h > p.h || (h == p.h && d > p.d);
+    }
+  };
+
+  template <typename N, typename E, typename C>
+  struct CostFunc : public ShortestPath::CostFunc<N, E, C> {
+    C operator()(const Node<N, E>* from, const Edge<N, E>* e,
+                 const Node<N, E>* to) const {
+      UNUSED(from);
+      UNUSED(e);
+      UNUSED(to);
+      return C();
+    };
+  };
+
+  template <typename N, typename E, typename C>
+  struct HeurFunc : public ShortestPath::HeurFunc<N, E, C> {
+    C operator()(const Node<N, E>* from,
+                 const std::set<Node<N, E>*>& to) const {
+      UNUSED(from);
+      UNUSED(to);
+      return C();
+    };
+  };
+
+  template <typename N, typename E, typename C>
+  using Settled = std::unordered_map<Edge<N, E>*, RouteEdge<N, E, C> >;
+
+  template <typename N, typename E, typename C>
+  using PQ = std::priority_queue<RouteEdge<N, E, C> >;
+
+  template <typename N, typename E, typename C>
+  static C shortestPathImpl(const std::set<Edge<N, E>*> from,
+                            const std::set<Edge<N, E>*>& to,
+                            const ShortestPath::CostFunc<N, E, C>& costFunc,
+                            const ShortestPath::HeurFunc<N, E, C>& heurFunc,
+                            EList<N, E>* resEdges, NList<N, E>* resNodes);
+
+  template <typename N, typename E, typename C>
+  static C shortestPathImpl(Node<N, E>* from, const std::set<Node<N, E>*>& to,
+                            const ShortestPath::CostFunc<N, E, C>& costFunc,
+                            const ShortestPath::HeurFunc<N, E, C>& heurFunc,
+                            EList<N, E>* resEdges, NList<N, E>* resNodes);
+
+  template <typename N, typename E, typename C>
+  static C shortestPathImpl(Edge<N, E>* from, const std::set<Node<N, E>*>& to,
+                            const ShortestPath::CostFunc<N, E, C>& costFunc,
+                            const ShortestPath::HeurFunc<N, E, C>& heurFunc,
+                            EList<N, E>* resEdges, NList<N, E>* resNodes);
+
+  template <typename N, typename E, typename C>
+  static C shortestPathImpl(const std::set<Edge<N, E>*>& from,
+                            const std::set<Node<N, E>*>& to,
+                            const ShortestPath::CostFunc<N, E, C>& costFunc,
+                            const ShortestPath::HeurFunc<N, E, C>& heurFunc,
+                            EList<N, E>* resEdges, NList<N, E>* resNodes);
+
+  template <typename N, typename E, typename C>
+  static std::unordered_map<Edge<N, E>*, C> shortestPathImpl(
+      const std::set<Edge<N, E>*>& from,
+      const ShortestPath::CostFunc<N, E, C>& costFunc, bool rev);
+
+  template <typename N, typename E, typename C>
+  static std::unordered_map<Edge<N, E>*, C> shortestPathImpl(
+      Edge<N, E>* from, const std::set<Edge<N, E>*>& to,
+      const ShortestPath::CostFunc<N, E, C>& costFunc,
+      const ShortestPath::HeurFunc<N, E, C>& heurFunc,
+      std::unordered_map<Edge<N, E>*, EList<N, E>*> resEdges,
+      std::unordered_map<Edge<N, E>*, NList<N, E>*> resNodes);
+
+  template <typename N, typename E, typename C>
+  static void buildPath(Edge<N, E>* curE, const Settled<N, E, C>& settled,
+                        NList<N, E>* resNodes, EList<N, E>* resEdges);
+
+  template <typename N, typename E, typename C>
+  static inline void relax(RouteEdge<N, E, C>& cur,
+                           const std::set<Edge<N, E>*>& to,
+                           const ShortestPath::CostFunc<N, E, C>& costFunc,
+                           const ShortestPath::HeurFunc<N, E, C>& heurFunc,
+                           PQ<N, E, C>& pq);
+
+  template <typename N, typename E, typename C>
+  static void relaxInv(RouteEdge<N, E, C>& cur,
+                      const ShortestPath::CostFunc<N, E, C>& costFunc,
+                      PQ<N, E, C>& pq);
+
+  static size_t ITERS;
+};
+
+#include "util/graph/EDijkstra.tpp"
+}
+}
+
+#endif  // UTIL_GRAPH_DIJKSTRA_H_

src/util/graph/EDijkstra.tpp

@@ -0,0 +1,258 @@
+// Copyright 2017, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+// _____________________________________________________________________________
+template <typename N, typename E, typename C>
+C EDijkstra::shortestPathImpl(Node<N, E>* from, const std::set<Node<N, E>*>& to,
+                              const ShortestPath::CostFunc<N, E, C>& costFunc,
+                              const ShortestPath::HeurFunc<N, E, C>& heurFunc,
+                              EList<N, E>* resEdges, NList<N, E>* resNodes) {
+  std::set<Edge<N, E>*> frEs;
+  std::set<Edge<N, E>*> toEs;
+
+  frEs.insert(from->getAdjListOut().begin(), from->getAdjListOut().end());
+
+  for (auto n : to) {
+    toEs.insert(n->getAdjListIn().begin(), n->getAdjListIn().end());
+  }
+
+  C cost = shortestPathImpl(frEs, toEs, costFunc, heurFunc, resEdges, resNodes);
+
+  // the beginning node is not included in our edge based dijkstra
+  if (resNodes) resNodes->push_back(from);
+
+  return cost;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E, typename C>
+C EDijkstra::shortestPathImpl(Edge<N, E>* from, const std::set<Node<N, E>*>& to,
+                              const ShortestPath::CostFunc<N, E, C>& costFunc,
+                              const ShortestPath::HeurFunc<N, E, C>& heurFunc,
+                              EList<N, E>* resEdges, NList<N, E>* resNodes) {
+  std::set<Edge<N, E>*> frEs;
+  std::set<Edge<N, E>*> toEs;
+
+  frEs.insert(from);
+
+  for (auto n : to) {
+    toEs.insert(n->getAdjListIn().begin(), n->getAdjListIn().end());
+  }
+
+  C cost = shortestPathImpl(frEs, toEs, costFunc, heurFunc, resEdges, resNodes);
+
+  return cost;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E, typename C>
+C EDijkstra::shortestPathImpl(const std::set<Edge<N, E>*> from,
+                              const std::set<Edge<N, E>*>& to,
+                              const ShortestPath::CostFunc<N, E, C>& costFunc,
+                              const ShortestPath::HeurFunc<N, E, C>& heurFunc,
+                              EList<N, E>* resEdges, NList<N, E>* resNodes) {
+  if (from.size() == 0 || to.size() == 0) return costFunc.inf();
+
+  Settled<N, E, C> settled;
+  PQ<N, E, C> pq;
+  bool found = false;
+
+  // at the beginning, put all edges on the priority queue,
+  // init them with their own cost
+  for (auto e : from) {
+    C c = costFunc(0, 0, e);
+    C h = heurFunc(e, to);
+    pq.emplace(e, (Edge<N, E>*)0, (Node<N, E>*)0, c, c + h);
+  }
+
+  RouteEdge<N, E, C> cur;
+
+  while (!pq.empty()) {
+    EDijkstra::ITERS++;
+
+    if (settled.find(pq.top().e) != settled.end()) {
+      pq.pop();
+      continue;
+    }
+
+    cur = pq.top();
+    pq.pop();
+
+    settled[cur.e] = cur;
+
+    if (to.find(cur.e) != to.end()) {
+      found = true;
+      break;
+    }
+
+    relax(cur, to, costFunc, heurFunc, pq);
+  }
+
+  if (!found) return costFunc.inf();
+
+  buildPath(cur.e, settled, resNodes, resEdges);
+
+  return cur.d;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E, typename C>
+std::unordered_map<Edge<N, E>*, C> EDijkstra::shortestPathImpl(
+    const std::set<Edge<N, E>*>& from, const ShortestPath::CostFunc<N, E, C>& costFunc,
+    bool rev) {
+  std::unordered_map<Edge<N, E>*, C> costs;
+
+  Settled<N, E, C> settled;
+  PQ<N, E, C> pq;
+
+  std::set<Edge<N, E>*> to;
+
+  for (auto e : from) {
+    pq.emplace(e, (Edge<N, E>*)0, (Node<N, E>*)0, costFunc(0, 0, e), C());
+  }
+
+  RouteEdge<N, E, C> cur;
+
+  while (!pq.empty()) {
+    EDijkstra::ITERS++;
+
+    if (settled.find(pq.top().e) != settled.end()) {
+      pq.pop();
+      continue;
+    }
+
+    cur = pq.top();
+    pq.pop();
+
+    settled[cur.e] = cur;
+
+    costs[cur.e] = cur.d;
+    buildPath(cur.e, settled, (NList<N, E>*)0, (EList<N, E>*)0);
+
+    if (rev)
+      relaxInv(cur, costFunc, pq);
+    else
+      relax(cur, to, costFunc, ZeroHeurFunc<N, E, C>(), pq);
+  }
+
+  return costs;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E, typename C>
+std::unordered_map<Edge<N, E>*, C> EDijkstra::shortestPathImpl(
+    Edge<N, E>* from, const std::set<Edge<N, E>*>& to,
+    const ShortestPath::CostFunc<N, E, C>& costFunc,
+    const ShortestPath::HeurFunc<N, E, C>& heurFunc,
+    std::unordered_map<Edge<N, E>*, EList<N, E>*> resEdges,
+    std::unordered_map<Edge<N, E>*, NList<N, E>*> resNodes) {
+  std::unordered_map<Edge<N, E>*, C> costs;
+  if (to.size() == 0) return costs;
+
+  // init costs with inf
+  for (auto e : to) costs[e] = costFunc.inf();
+
+  Settled<N, E, C> settled;
+  PQ<N, E, C> pq;
+
+  size_t found = 0;
+
+  C c = costFunc(0, 0, from);
+  C h = heurFunc(from, to);
+  pq.emplace(from, (Edge<N, E>*)0, (Node<N, E>*)0, c, c + h);
+
+  RouteEdge<N, E, C> cur;
+
+  while (!pq.empty()) {
+    EDijkstra::ITERS++;
+
+    if (settled.find(pq.top().e) != settled.end()) {
+      pq.pop();
+      continue;
+    }
+
+    cur = pq.top();
+    pq.pop();
+
+    settled[cur.e] = cur;
+
+    if (to.find(cur.e) != to.end()) {
+      found++;
+      costs[cur.e] = cur.d;
+      buildPath(cur.e, settled, resNodes[cur.e], resEdges[cur.e]);
+    }
+
+    if (found == to.size()) return costs;
+
+    relax(cur, to, costFunc, heurFunc, pq);
+  }
+
+  return costs;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E, typename C>
+void EDijkstra::relaxInv(RouteEdge<N, E, C>& cur,
+                         const ShortestPath::CostFunc<N, E, C>& costFunc,
+                         PQ<N, E, C>& pq) {
+
+  // handling undirected graph makes no sense here
+
+  for (const auto edge : cur.e->getFrom()->getAdjListIn()) {
+    if (edge == cur.e) continue;
+    C newC = costFunc(edge, cur.e->getFrom(), cur.e);
+    newC = cur.d + newC;
+    if (costFunc.inf() <= newC) continue;
+
+    pq.emplace(edge, cur.e, cur.e->getFrom(), newC, C());
+  }
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E, typename C>
+void EDijkstra::relax(RouteEdge<N, E, C>& cur, const std::set<Edge<N, E>*>& to,
+                      const ShortestPath::CostFunc<N, E, C>& costFunc,
+                      const ShortestPath::HeurFunc<N, E, C>& heurFunc,
+                      PQ<N, E, C>& pq) {
+  if (cur.e->getFrom()->hasEdgeIn(cur.e)) {
+    // for undirected graphs
+    for (const auto edge : cur.e->getFrom()->getAdjListOut()) {
+      if (edge == cur.e) continue;
+      C newC = costFunc(cur.e, cur.e->getFrom(), edge);
+      newC = cur.d + newC;
+      if (costFunc.inf() <= newC) continue;
+
+      const C& h = heurFunc(edge, to);
+      const C& newH = newC + h;
+
+      pq.emplace(edge, cur.e, cur.e->getFrom(), newC, newH);
+    }
+  }
+
+  for (const auto edge : cur.e->getTo()->getAdjListOut()) {
+    if (edge == cur.e) continue;
+    C newC = costFunc(cur.e, cur.e->getTo(), edge);
+    newC = cur.d + newC;
+    if (costFunc.inf() <= newC) continue;
+
+    const C& h = heurFunc(edge, to);
+    const C& newH = newC + h;
+
+    pq.emplace(edge, cur.e, cur.e->getTo(), newC, newH);
+  }
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E, typename C>
+void EDijkstra::buildPath(Edge<N, E>* curE, const Settled<N, E, C>& settled,
+                          NList<N, E>* resNodes, EList<N, E>* resEdges) {
+  const RouteEdge<N, E, C>* curEdge = &settled.find(curE)->second;
+  if (resNodes) resNodes->push_back(curEdge->e->getOtherNd(curEdge->n));
+  while (true) {
+    if (resNodes && curEdge->n) resNodes->push_back(curEdge->n);
+    if (resEdges) resEdges->push_back(curEdge->e);
+    if (!curEdge->parent) break;
+    curEdge = &settled.find(curEdge->parent)->second;
+  }
+}

src/util/graph/Edge.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_GRAPH_EDGE_H_
+#define UTIL_GRAPH_EDGE_H_
+
+#include <vector>
+#include "util/graph/Node.h"
+
+namespace util {
+namespace graph {
+
+template <typename N, typename E>
+class Edge {
+ public:
+  Edge(Node<N, E>* from, Node<N, E>* to, const E& pl);
+
+  Node<N, E>* getFrom() const;
+  Node<N, E>* getTo() const;
+
+  Node<N, E>* getOtherNd(const Node<N, E>* notNode) const;
+
+  void setFrom(Node<N, E>* from);
+  void setTo(Node<N, E>* to);
+
+  E& pl();
+  const E& pl() const;
+
+ private:
+  Node<N, E>* _from;
+  Node<N, E>* _to;
+  E _pl;
+};
+
+#include "util/graph/Edge.tpp"
+
+}}
+
+#endif  // UTIL_GRAPH_EDGE_H_
+

src/util/graph/Edge.tpp

@@ -0,0 +1,41 @@
+// Copyright 2016, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+Edge<N, E>::Edge(Node<N, E>* from, Node<N, E>* to, const E& pl)
+ : _from(from), _to(to), _pl(pl) {
+
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+Node<N, E>* Edge<N, E>::getFrom() const {
+  return _from;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+Node<N, E>* Edge<N, E>::getTo() const {
+  return _to;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+Node<N, E>* Edge<N, E>::getOtherNd(const Node<N, E>* notNode) const {
+  if (_to == notNode) return _from;
+  return _to;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+E& Edge<N, E>::pl() {
+  return _pl;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+const E& Edge<N, E>::pl() const {
+  return _pl;
+}

src/util/graph/Graph.h

@@ -0,0 +1,47 @@
+// Copyright 2016, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+#ifndef UTIL_GRAPH_GRAPH_H_
+#define UTIL_GRAPH_GRAPH_H_
+
+#include <set>
+#include <string>
+#include <iostream>
+
+#include "util/graph/Edge.h"
+#include "util/graph/Node.h"
+
+namespace util {
+namespace graph {
+
+template <typename N, typename E>
+class Graph {
+ public:
+  ~Graph();
+  virtual Node<N, E>* addNd() = 0;
+  virtual Node<N, E>* addNd(const N& pl) = 0;
+  Edge<N, E>* addEdg(Node<N, E>* from, Node<N, E>* to);
+  virtual Edge<N, E>* addEdg(Node<N, E>* from, Node<N, E>* to, const E& p) = 0;
+  Edge<N, E>* getEdg(Node<N, E>* from, Node<N, E>* to);
+  const Edge<N, E>* getEdg(Node<N, E>* from, Node<N, E>* to) const;
+
+  virtual Node<N, E>* mergeNds(Node<N, E>* a, Node<N, E>* b) = 0;
+
+  const std::set<Node<N, E>*>& getNds() const;
+  std::set<Node<N, E>*>* getNds();
+
+  typename std::set<Node<N, E>*>::iterator delNd(Node<N, E>* n);
+  typename std::set<Node<N, E>*>::iterator delNd(
+      typename std::set<Node<N, E>*>::iterator i);
+  void delEdg(Node<N, E>* from, Node<N, E>* to);
+
+ private:
+  std::set<Node<N, E>*> _nodes;
+};
+
+#include "util/graph/Graph.tpp"
+}
+}
+
+#endif  // UTIL_GRAPH_GRAPH_H_

src/util/graph/Graph.tpp

@@ -0,0 +1,76 @@
+// Copyright 2016, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+Graph<N, E>::~Graph() {
+  for (auto n : _nodes) delete n;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+Edge<N, E>* Graph<N, E>::addEdg(Node<N, E>* from, Node<N, E>* to) {
+  return addEdg(from, to, E());
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+const std::set<Node<N, E>*>& Graph<N, E>::getNds() const {
+  return _nodes;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+std::set<Node<N, E>*>* Graph<N, E>::getNds() {
+  return &_nodes;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+typename std::set<Node<N, E>*>::iterator Graph<N, E>::delNd(
+    Node<N, E>* n) {
+  return delNd(_nodes.find(n));
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+typename std::set<Node<N, E>*>::iterator Graph<N, E>::delNd(
+    typename std::set<Node<N, E>*>::iterator i) {
+  delete *i;
+  return _nodes.erase(i);
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+void Graph<N, E>::delEdg(Node<N, E>* from, Node<N, E>* to) {
+  Edge<N, E>* toDel = getEdg(from, to);
+  if (!toDel) return;
+
+  from->removeEdge(toDel);
+  to->removeEdge(toDel);
+
+  assert(!getEdg(from, to));
+
+  delete toDel;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+Edge<N, E>* Graph<N, E>::getEdg(Node<N, E>* from, Node<N, E>* to) {
+  for (auto e : from->getAdjList()) {
+    if (e->getOtherNd(from) == to) return e;
+  }
+
+  return 0;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+const Edge<N, E>* Graph<N, E>::getEdg(Node<N, E>* from, Node<N, E>* to) const {
+  for (auto e : from->getAdjList()) {
+    if (e->getOtherNd(from) == to) return e;
+  }
+
+  return 0;
+}

src/util/graph/Node.h

@@ -0,0 +1,44 @@
+// Copyright 2016, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+#ifndef UTIL_GRAPH_NODE_H_
+#define UTIL_GRAPH_NODE_H_
+
+#include <vector>
+
+namespace util {
+namespace graph {
+
+// forward declaration of Edge
+template <typename N, typename E>
+class Edge;
+
+template <typename N, typename E>
+class Node {
+ public:
+  virtual const std::vector<Edge<N, E>*>& getAdjList() const = 0;
+  virtual const std::vector<Edge<N, E>*>& getAdjListOut() const = 0;
+  virtual const std::vector<Edge<N, E>*>& getAdjListIn() const = 0;
+
+  virtual size_t getDeg() const = 0;
+  virtual size_t getInDeg() const = 0;
+  virtual size_t getOutDeg() const = 0;
+
+  virtual bool hasEdgeIn(const Edge<N, E>* e) const = 0;
+  virtual bool hasEdgeOut(const Edge<N, E>* e) const = 0;
+  virtual bool hasEdge(const Edge<N, E>* e) const = 0;
+
+  // add edge to this node's adjacency lists
+  virtual void addEdge(Edge<N, E>* e) = 0;
+  virtual void removeEdge(Edge<N, E>* e) = 0;
+
+  virtual ~Node() {};
+
+  virtual N& pl() = 0;
+  virtual const N& pl() const = 0;
+};
+
+}}
+
+#endif  // UTIL_GRAPH_NODE_H_

src/util/graph/ShortestPath.h

@@ -0,0 +1,401 @@
+// Copyright 2017, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+#ifndef UTIL_GRAPH_SHORTESTPATH_H_
+#define UTIL_GRAPH_SHORTESTPATH_H_
+
+#include <exception>
+#include <iostream>
+#include <limits>
+#include <list>
+#include <queue>
+#include <set>
+#include <unordered_map>
+#include "util/graph/Edge.h"
+#include "util/graph/Graph.h"
+#include "util/graph/Node.h"
+
+namespace util {
+namespace graph {
+
+using util::graph::Graph;
+using util::graph::Node;
+using util::graph::Edge;
+
+// shortest path base class
+template <class D>
+class ShortestPath {
+ public:
+  template <typename N, typename E>
+  using EList = std::vector<Edge<N, E>*>;
+
+  template <typename N, typename E>
+  using NList = std::vector<Node<N, E>*>;
+
+  template <typename N, typename E, typename C>
+  struct CostFunc {
+    virtual C operator()(const Node<N, E>* from, const Edge<N, E>* e,
+                         const Node<N, E>* to) const = 0;
+    virtual C operator()(const Edge<N, E>* from, const Node<N, E>* n,
+                         const Edge<N, E>* to) const = 0;
+    virtual C inf() const = 0;
+  };
+
+  template <typename N, typename E, typename C>
+  struct HeurFunc {
+    virtual C operator()(const Node<N, E>* a,
+                         const std::set<Node<N, E>*>& b) const = 0;
+    virtual C operator()(const Edge<N, E>* a,
+                         const std::set<Edge<N, E>*>& b) const = 0;
+  };
+
+  template <typename N, typename E, typename C>
+  struct ZeroHeurFunc : public HeurFunc<N, E, C> {
+    C operator()(const Node<N, E>* a, const std::set<Node<N, E>*>& b) const {
+      UNUSED(a);
+      UNUSED(b);
+      return C();
+    }
+    C operator()(const Edge<N, E>* a, const std::set<Edge<N, E>*>& b) const {
+      UNUSED(a);
+      UNUSED(b);
+      return C();
+    }
+  };
+
+  template <typename N, typename E, typename C>
+  static C shortestPath(Node<N, E>* from, const std::set<Node<N, E>*>& to,
+                        const CostFunc<N, E, C>& costFunc,
+                        const HeurFunc<N, E, C>& heurFunc,
+                        EList<N, E>* resEdges, NList<N, E>* resNodes) {
+    return D::shortestPathImpl(from, to, costFunc, heurFunc, resEdges,
+                               resNodes);
+  }
+
+  template <typename N, typename E, typename C>
+  static C shortestPath(const std::set<Node<N, E>*> from, const std::set<Node<N, E>*>& to,
+                        const CostFunc<N, E, C>& costFunc,
+                        const HeurFunc<N, E, C>& heurFunc,
+                        EList<N, E>* resEdges, NList<N, E>* resNodes) {
+    return D::shortestPathImpl(from, to, costFunc, heurFunc, resEdges,
+                               resNodes);
+  }
+
+  template <typename N, typename E, typename C>
+  static C shortestPath(Node<N, E>* from, const std::set<Node<N, E>*>& to,
+                        const CostFunc<N, E, C>& costFunc,
+                        EList<N, E>* resEdges, NList<N, E>* resNodes) {
+    return D::shortestPathImpl(from, to, costFunc, ZeroHeurFunc<N, E, C>(),
+                               resEdges, resNodes);
+  }
+
+  template <typename N, typename E, typename C>
+  static std::unordered_map<Node<N, E>*, C> shortestPath(
+      Node<N, E>* from, const std::set<Node<N, E>*>& to,
+      const CostFunc<N, E, C>& costFunc, const HeurFunc<N, E, C>& heurFunc,
+      std::unordered_map<Node<N, E>*, EList<N, E>*> resEdges,
+      std::unordered_map<Node<N, E>*, NList<N, E>*> resNodes) {
+    return D::shortestPathImpl(from, to, costFunc, heurFunc, resEdges,
+                               resNodes);
+  }
+
+  template <typename N, typename E, typename C>
+  static std::unordered_map<Node<N, E>*, C> shortestPath(
+      Node<N, E>* from, const std::set<Node<N, E>*>& to,
+      const CostFunc<N, E, C>& costFunc,
+      std::unordered_map<Node<N, E>*, EList<N, E>*> resEdges,
+      std::unordered_map<Node<N, E>*, NList<N, E>*> resNodes) {
+    return D::shortestPathImpl(from, to, costFunc, ZeroHeurFunc<N, E, C>(),
+                               resEdges, resNodes);
+  }
+
+  template <typename N, typename E, typename C>
+  static std::unordered_map<Node<N, E>*, C> shortestPath(
+      Node<N, E>* from, const std::set<Node<N, E>*>& to,
+      const CostFunc<N, E, C>& costFunc, const HeurFunc<N, E, C>& heurFunc,
+      std::unordered_map<Node<N, E>*, EList<N, E>*> resEdges) {
+    std::unordered_map<Node<N, E>*, NList<N, E>*> dummyRet;
+    return D::shortestPathImpl(from, to, costFunc, heurFunc, resEdges,
+                               dummyRet);
+  }
+
+  template <typename N, typename E, typename C>
+  static std::unordered_map<Node<N, E>*, C> shortestPath(
+      Node<N, E>* from, const std::set<Node<N, E>*>& to,
+      const CostFunc<N, E, C>& costFunc,
+      std::unordered_map<Node<N, E>*, EList<N, E>*> resEdges) {
+    std::unordered_map<Node<N, E>*, NList<N, E>*> dummyRet;
+    return D::shortestPathImpl(from, to, costFunc, ZeroHeurFunc<N, E, C>(),
+                               resEdges, dummyRet);
+  }
+
+  template <typename N, typename E, typename C>
+  static std::unordered_map<Node<N, E>*, C> shortestPath(
+      Node<N, E>* from, const std::set<Node<N, E>*>& to,
+      const CostFunc<N, E, C>& costFunc,
+      std::unordered_map<Node<N, E>*, NList<N, E>*> resNodes) {
+    std::unordered_map<Node<N, E>*, EList<N, E>*> dummyRet;
+    return D::shortestPathImpl(from, to, costFunc, ZeroHeurFunc<N, E, C>(),
+                               dummyRet, resNodes);
+  }
+
+  template <typename N, typename E, typename C>
+  static std::unordered_map<Node<N, E>*, C> shortestPath(
+      Node<N, E>* from, const std::set<Node<N, E>*>& to,
+      const CostFunc<N, E, C>& costFunc, const HeurFunc<N, E, C>& heurFunc,
+      std::unordered_map<Node<N, E>*, NList<N, E>*> resNodes) {
+    std::unordered_map<Node<N, E>*, EList<N, E>*> dummyRet;
+    return D::shortestPathImpl(from, to, costFunc, heurFunc, dummyRet,
+                               resNodes);
+  }
+
+  template <typename N, typename E, typename C>
+  static C shortestPath(Node<N, E>* from, Node<N, E>* to,
+                        const CostFunc<N, E, C>& costFunc,
+                        EList<N, E>* resEdges, NList<N, E>* resNodes) {
+    if (to->getInDeg() == 0) return costFunc.inf();
+    std::set<Node<N, E>*> tos;
+    tos.insert(to);
+    return shortestPath(from, tos, costFunc, resEdges, resNodes);
+  }
+
+  template <typename N, typename E, typename C>
+  static C shortestPath(Node<N, E>* from, Node<N, E>* to,
+                        const CostFunc<N, E, C>& costFunc) {
+    if (to->getInDeg() == 0) return costFunc.inf();
+    std::set<Node<N, E>*> tos;
+    tos.insert(to);
+    EList<N, E>* el = 0;
+    NList<N, E>* nl = 0;
+    return shortestPath(from, tos, costFunc, el, nl);
+  }
+
+  template <typename N, typename E, typename C>
+  static C shortestPath(Node<N, E>* from, Node<N, E>* to,
+                        const CostFunc<N, E, C>& costFunc,
+                        NList<N, E>* resNodes) {
+    if (to->getInDeg() == 0) return costFunc.inf();
+    std::set<Node<N, E>*> tos;
+    tos.insert(to);
+    EList<N, E>* el = 0;
+    return shortestPath(from, tos, costFunc, el, resNodes);
+  }
+
+  template <typename N, typename E, typename C>
+  static C shortestPath(Node<N, E>* from, Node<N, E>* to,
+                        const CostFunc<N, E, C>& costFunc,
+                        EList<N, E>* resEdges) {
+    if (to->getInDeg() == 0) return costFunc.inf();
+    std::set<Node<N, E>*> tos;
+    tos.insert(to);
+    NList<N, E>* nl = 0;
+    return shortestPath(from, tos, costFunc, resEdges, nl);
+  }
+
+  template <typename N, typename E, typename C>
+  static C shortestPath(Node<N, E>* from, const std::set<Node<N, E>*>& to,
+                        const CostFunc<N, E, C>& costFunc,
+                        NList<N, E>* resNodes) {
+    EList<N, E>* el = 0;
+    return shortestPath(from, to, costFunc, el, resNodes);
+  }
+
+  template <typename N, typename E, typename C>
+  static C shortestPath(Node<N, E>* from, const std::set<Node<N, E>*>& to,
+                        const CostFunc<N, E, C>& costFunc,
+                        EList<N, E>* resEdges) {
+    NList<N, E>* nl = 0;
+    return shortestPath(from, to, costFunc, resEdges, nl);
+  }
+
+  template <typename N, typename E, typename C>
+  static C shortestPath(Node<N, E>* from, Node<N, E>* to,
+                        const CostFunc<N, E, C>& costFunc,
+                        const HeurFunc<N, E, C>& heurFunc,
+                        EList<N, E>* resEdges, NList<N, E>* resNodes) {
+    if (to->getInDeg() == 0) return costFunc.inf();
+    std::set<Node<N, E>*> tos;
+    tos.insert(to);
+    return D::shortestPathImpl(from, tos, costFunc, heurFunc, resEdges,
+                               resNodes);
+  }
+
+  template <typename N, typename E, typename C>
+  static C shortestPath(Node<N, E>* from, Node<N, E>* to,
+                        const CostFunc<N, E, C>& costFunc,
+                        const HeurFunc<N, E, C>& heurFunc,
+                        NList<N, E>* resNodes) {
+    if (to->getInDeg() == 0) return costFunc.inf();
+    std::set<Node<N, E>*> tos;
+    tos.insert(to);
+    EList<N, E>* el = 0;
+    return D::shortestPathImpl(from, tos, costFunc, heurFunc, el, resNodes);
+  }
+
+  template <typename N, typename E, typename C>
+  static C shortestPath(Node<N, E>* from, Node<N, E>* to,
+                        const CostFunc<N, E, C>& costFunc,
+                        const HeurFunc<N, E, C>& heurFunc,
+                        EList<N, E>* resEdges) {
+    if (to->getInDeg() == 0) return costFunc.inf();
+    std::set<Node<N, E>*> tos;
+    tos.insert(to);
+    NList<N, E>* nl = 0;
+    return D::shortestPathImpl(from, tos, costFunc, heurFunc, resEdges, nl);
+  }
+
+  template <typename N, typename E, typename C>
+  static C shortestPath(Node<N, E>* from, const std::set<Node<N, E>*>& to,
+                        const CostFunc<N, E, C>& costFunc,
+                        const HeurFunc<N, E, C>& heurFunc,
+                        NList<N, E>* resNodes) {
+    EList<N, E>* el = 0;
+    return D::shortestPathImpl(from, to, costFunc, heurFunc, el, resNodes);
+  }
+
+  template <typename N, typename E, typename C>
+  static C shortestPath(Node<N, E>* from, const std::set<Node<N, E>*>& to,
+                        const CostFunc<N, E, C>& costFunc,
+                        const HeurFunc<N, E, C>& heurFunc,
+                        EList<N, E>* resEdges) {
+    NList<N, E>* nl = 0;
+    return D::shortestPathImpl(from, to, costFunc, heurFunc, resEdges, nl);
+  }
+
+  template <typename N, typename E, typename C>
+  static std::unordered_map<Edge<N, E>*, C> shortestPath(
+      Edge<N, E>* from, const std::set<Edge<N, E>*>& to,
+      const ShortestPath::CostFunc<N, E, C>& costFunc,
+      const HeurFunc<N, E, C>& heurFunc,
+      std::unordered_map<Edge<N, E>*, EList<N, E>*> resEdges,
+      std::unordered_map<Edge<N, E>*, NList<N, E>*> resNodes) {
+    return D::shortestPathImpl(from, to, costFunc, heurFunc, resEdges,
+                               resNodes);
+  }
+
+  template <typename N, typename E, typename C>
+  static std::unordered_map<Edge<N, E>*, C> shortestPath(
+      Edge<N, E>* from, const std::set<Edge<N, E>*>& to,
+      const ShortestPath::CostFunc<N, E, C>& costFunc,
+      const HeurFunc<N, E, C>& heurFunc,
+      std::unordered_map<Edge<N, E>*, EList<N, E>*> resEdges) {
+    std::unordered_map<Edge<N, E>*, NList<N, E>*> dummyRet;
+    return D::shortestPathImpl(from, to, costFunc, heurFunc, resEdges,
+                               dummyRet);
+  }
+
+  template <typename N, typename E, typename C>
+  static std::unordered_map<Edge<N, E>*, C> shortestPath(
+      Edge<N, E>* from, const std::set<Edge<N, E>*>& to,
+      const ShortestPath::CostFunc<N, E, C>& costFunc,
+      const HeurFunc<N, E, C>& heurFunc) {
+    std::unordered_map<Edge<N, E>*, NList<N, E>*> dummyRet;
+    std::unordered_map<Edge<N, E>*, EList<N, E>*> dummyRetE;
+    return D::shortestPathImpl(from, to, costFunc, heurFunc, dummyRetE,
+                               dummyRet);
+  }
+
+  template <typename N, typename E, typename C>
+  static C shortestPath(const std::set<Edge<N, E>*>& from,
+                        const std::set<Edge<N, E>*>& to,
+                        const CostFunc<N, E, C>& costFunc,
+                        const HeurFunc<N, E, C>& heurFunc) {
+    NList<N, E>* nl = 0;
+    EList<N, E>* el = 0;
+    return D::shortestPathImpl(from, to, costFunc, heurFunc, el, nl);
+  }
+
+  template <typename N, typename E, typename C>
+  static C shortestPath(Edge<N, E>* from,
+                        Edge<N, E>* to,
+                        const CostFunc<N, E, C>& costFunc ) {
+    NList<N, E>* nl = 0;
+    EList<N, E>* el = 0;
+    std::set<Edge<N, E>*> tos{to};
+    std::set<Edge<N, E>*> froms{from};
+    return D::shortestPathImpl(froms, tos, costFunc, ZeroHeurFunc<N, E, C>(), el, nl);
+  }
+
+  template <typename N, typename E, typename C>
+  static C shortestPath(const std::set<Edge<N, E>*>& from,
+                        const std::set<Edge<N, E>*>& to,
+                        const CostFunc<N, E, C>& costFunc,
+                        const HeurFunc<N, E, C>& heurFunc, EList<N, E>* el) {
+    NList<N, E>* nl = 0;
+    return D::shortestPathImpl(from, to, costFunc, heurFunc, el, nl);
+  }
+
+  template <typename N, typename E, typename C>
+  static C shortestPath(Edge<N, E>* from, const std::set<Node<N, E>*>& to,
+                        const CostFunc<N, E, C>& costFunc,
+                        const HeurFunc<N, E, C>& heurFunc, EList<N, E>* el,
+                        NList<N, E>* nl) {
+    return D::shortestPathImpl(from, to, costFunc, heurFunc, el, nl);
+  }
+
+  template <typename N, typename E, typename C>
+  static C shortestPath(Edge<N, E>* from, const std::set<Node<N, E>*>& to,
+                        const CostFunc<N, E, C>& costFunc,
+                        EList<N, E>* el,
+                        NList<N, E>* nl) {
+    return D::shortestPathImpl(from, to, costFunc, ZeroHeurFunc<N, E, C>(), el,
+                               nl);
+  }
+
+  template <typename N, typename E, typename C>
+  static C shortestPath(Edge<N, E>* from, Node<N, E>* to,
+                        const CostFunc<N, E, C>& costFunc,
+                        const HeurFunc<N, E, C>& heurFunc, EList<N, E>* el,
+                        NList<N, E>* nl) {
+    std::set<Node<N, E>*> tos{to};
+    return D::shortestPathImpl(from, tos, costFunc, heurFunc, el, nl);
+  }
+
+  template <typename N, typename E, typename C>
+  static C shortestPath(Edge<N, E>* from, Node<N, E>* to,
+                        const CostFunc<N, E, C>& costFunc, EList<N, E>* el,
+                        NList<N, E>* nl) {
+    std::set<Node<N, E>*> tos{to};
+    return D::shortestPathImpl(from, tos, costFunc, ZeroHeurFunc<N, E, C>(), el,
+                               nl);
+  }
+
+  template <typename N, typename E, typename C>
+  static std::unordered_map<Edge<N, E>*, C> shortestPath(
+      Edge<N, E>* from,
+      const ShortestPath::CostFunc<N, E, C>& costFunc) {
+    std::set<Edge<N, E>*> froms { from };
+    return D::shortestPathImpl(froms, costFunc, false);
+  }
+
+  template <typename N, typename E, typename C>
+  static std::unordered_map<Edge<N, E>*, C> shortestPathRev(
+      Edge<N, E>* from,
+      const ShortestPath::CostFunc<N, E, C>& costFunc) {
+    std::set<Edge<N, E>*> froms { from };
+    return D::shortestPathImpl(froms, costFunc, true);
+  }
+
+  template <typename N, typename E, typename C>
+  static std::unordered_map<Edge<N, E>*, C> shortestPath(
+      Node<N, E>* from,
+      const ShortestPath::CostFunc<N, E, C>& costFunc) {
+    std::set<Edge<N, E>*> froms;
+    froms.insert(from->getAdjListOut().begin(), from->getAdjListOut().end());
+    return D::shortestPathImpl(froms, costFunc, false);
+  }
+
+  template <typename N, typename E, typename C>
+  static std::unordered_map<Edge<N, E>*, C> shortestPathRev(
+      Node<N, E>* from,
+      const ShortestPath::CostFunc<N, E, C>& costFunc) {
+    std::set<Edge<N, E>*> froms;
+    froms.insert(from->getAdjListOut().begin(), from->getAdjListOut().end());
+    return D::shortestPathImpl(froms, costFunc, true);
+  }
+};
+}
+}
+
+#endif  // UTIL_GRAPH_SHORTESTPATH_H_

src/util/graph/ShortestPath.tpp

@@ -0,0 +1 @@
+

src/util/graph/UndirGraph.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_GRAPH_UNDIRGRAPH_H_
+#define UTIL_GRAPH_UNDIRGRAPH_H_
+
+#include <set>
+#include <string>
+
+#include "util/graph/Graph.h"
+#include "util/graph/Edge.h"
+#include "util/graph/UndirNode.h"
+
+namespace util {
+namespace graph {
+
+template <typename N, typename E>
+using UndirEdge = Edge<N, E>;
+
+template <typename N, typename E>
+class UndirGraph : public Graph<N, E> {
+ public:
+  explicit UndirGraph();
+
+  using Graph<N, E>::addEdg;
+
+  Node<N, E>* addNd();
+  Node<N, E>* addNd(UndirNode<N, E>* n);
+  Node<N, E>* addNd(const N& pl);
+  Edge<N, E>* addEdg(Node<N, E>* from, Node<N, E>* to, const E& p);
+
+  Node<N, E>* mergeNds(Node<N, E>* a, Node<N, E>* b);
+
+};
+
+#include "util/graph/UndirGraph.tpp"
+}
+}
+
+#endif  // UTIL_GRAPH_UNDIRGRAPH_H_

src/util/graph/UndirGraph.tpp

@@ -0,0 +1,59 @@
+// Copyright 2016, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+UndirGraph<N, E>::UndirGraph() {}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+Node<N, E>* UndirGraph<N, E>::addNd(const N& pl) {
+  return addNd(new UndirNode<N, E>(pl));
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+Node<N, E>* UndirGraph<N, E>::addNd() {
+  return addNd(new UndirNode<N, E>());
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+Node<N, E>* UndirGraph<N, E>::addNd(UndirNode<N, E>* n) {
+  auto ins = Graph<N, E>::getNds()->insert(n);
+  return *ins.first;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+Edge<N, E>* UndirGraph<N, E>::addEdg(Node<N, E>* from, Node<N, E>* to,
+                                     const E& p) {
+  Edge<N, E>* e = Graph<N, E>::getEdg(from, to);
+  if (!e) {
+    e = new Edge<N, E>(from, to, p);
+    from->addEdge(e);
+    to->addEdge(e);
+  }
+  return e;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+Node<N, E>* UndirGraph<N, E>::mergeNds(Node<N, E>* a, Node<N, E>* b) {
+  for (auto e : a->getAdjListOut()) {
+    if (e->getTo() != b) {
+      addEdg(b, e->getTo(), e->pl());
+    }
+  }
+
+  for (auto e : a->getAdjListIn()) {
+    if (e->getFrom() != b) {
+      addEdg(e->getFrom(), b, e->pl());
+    }
+  }
+
+  UndirGraph<N, E>::delNd(a);
+
+  return b;
+}

src/util/graph/UndirNode.h

@@ -0,0 +1,53 @@
+// Copyright 2016, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+#ifndef UTIL_GRAPH_UNDIRNODE_H_
+#define UTIL_GRAPH_UNDIRNODE_H_
+
+#include <vector>
+#include "util/graph/Node.h"
+
+namespace util {
+namespace graph {
+
+template <typename N, typename E>
+class UndirNode : public Node<N, E> {
+ public:
+  UndirNode();
+  UndirNode(const N& pl);
+  ~UndirNode();
+
+  const std::vector<Edge<N, E>*>& getAdjList() const;
+  const std::vector<Edge<N, E>*>& getAdjListIn() const;
+  const std::vector<Edge<N, E>*>& getAdjListOut() const;
+
+  size_t getDeg() const;
+  size_t getInDeg() const;
+  size_t getOutDeg() const;
+
+  bool hasEdgeIn(const Edge<N, E>* e) const;
+  bool hasEdgeOut(const Edge<N, E>* e) const;
+  bool hasEdge(const Edge<N, E>* e) const;
+
+  // add edge to this node's adjacency lists
+  void addEdge(Edge<N, E>* e);
+
+  // remove edge from this node's adjacency lists
+  void removeEdge(Edge<N, E>* e);
+
+  N& pl();
+  const N& pl() const;
+
+ private:
+  std::vector<Edge<N, E>*> _adjList;
+  N _pl;
+
+  bool adjContains(const Edge<N, E>* e) const;
+};
+
+#include "util/graph/UndirNode.tpp"
+
+}}
+
+#endif  // UTIL_GRAPH_UNDIRNODE_H_

src/util/graph/UndirNode.tpp

@@ -0,0 +1,130 @@
+// Copyright 2016, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+UndirNode<N, E>::UndirNode() : _pl() {
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+UndirNode<N, E>::UndirNode(const N& pl) : _pl(pl) {
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+UndirNode<N, E>::~UndirNode() {
+  // delete self edges
+  for (auto e = _adjList.begin(); e != _adjList.end();) {
+    Edge<N, E>* eP = *e;
+    if (eP->getTo() == this && eP->getFrom() == this) {
+      e = _adjList.erase(e);
+      delete eP;
+    } else {
+      e++;
+    }
+  }
+
+  for (auto e = _adjList.begin(); e != _adjList.end(); e++) {
+    Edge<N, E>* eP = *e;
+
+    if (eP->getTo() != this) {
+      eP->getTo()->removeEdge(eP);
+    }
+
+    if (eP->getFrom() != this) {
+      eP->getFrom()->removeEdge(eP);
+    }
+
+    delete eP;
+  }
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+bool UndirNode<N, E>::hasEdgeIn(const Edge<N, E>* e) const {
+  return hasEdge(e);
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+bool UndirNode<N, E>::hasEdgeOut(const Edge<N, E>* e) const {
+  return hasEdge(e);
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+bool UndirNode<N, E>::hasEdge(const Edge<N, E>* e) const {
+  return e->getFrom() == this || e->getTo() == this;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+bool UndirNode<N, E>::adjContains(const Edge<N, E>* e) const {
+  for (size_t i = 0; i < _adjList.size(); i++) if (_adjList[i] == e) return true;
+  return false;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+void UndirNode<N, E>::addEdge(Edge<N, E>* e) {
+  if (adjContains(e)) return;
+  _adjList.reserve(_adjList.size() + 1);
+  _adjList.push_back(e);
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+void UndirNode<N, E>::removeEdge(Edge<N, E>* e) {
+  auto p = std::find(_adjList.begin(), _adjList.end(), e);
+  if (p != _adjList.end()) _adjList.erase(p);
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+const std::vector<Edge<N, E>*>& UndirNode<N, E>::getAdjList() const {
+  return _adjList;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+const std::vector<Edge<N, E>*>& UndirNode<N, E>::getAdjListOut() const {
+  return _adjList;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+const std::vector<Edge<N, E>*>& UndirNode<N, E>::getAdjListIn() const {
+  return _adjList;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+size_t UndirNode<N, E>::getDeg() const {
+  return _adjList.size();
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+size_t UndirNode<N, E>::getInDeg() const {
+  return getDeg();
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+size_t UndirNode<N, E>::getOutDeg() const {
+  return getDeg();
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+N& UndirNode<N, E>::pl() {
+  return _pl;
+}
+
+// _____________________________________________________________________________
+template <typename N, typename E>
+const N& UndirNode<N, E>::pl() const {
+  return _pl;
+}

src/util/json/JsonWriter.cpp

@@ -0,0 +1,146 @@
+// Copyright 2018, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+#include <iomanip>
+#include "JsonWriter.h"
+#include "util/String.h"
+using namespace util;
+using namespace json;
+
+using std::ostream;
+using std::string;
+using std::map;
+
+// _____________________________________________________________________________
+JsonWriter::JsonWriter(std::ostream* out)
+    : _out(out), _pretty(false), _indent(2) {
+  *_out << std::setprecision(10);
+}
+
+// _____________________________________________________________________________
+JsonWriter::JsonWriter(std::ostream* out, size_t prec)
+    : _out(out), _pretty(false), _indent(2) {
+  *_out << std::setprecision(prec);
+}
+
+// _____________________________________________________________________________
+JsonWriter::JsonWriter(std::ostream* out, size_t prec, bool pret)
+    : _out(out), _pretty(pret), _indent(2) {
+  *_out << std::setprecision(prec);
+}
+
+// _____________________________________________________________________________
+JsonWriter::JsonWriter(std::ostream* out, size_t prec, bool pret, size_t indent)
+    : _out(out), _pretty(pret), _indent(indent) {
+  *_out << std::setprecision(prec);
+}
+
+// _____________________________________________________________________________
+void JsonWriter::obj() {
+  if (!_stack.empty() && _stack.top().type == OBJ)
+    throw JsonWriterException("Object not allowed as key");
+  if (!_stack.empty() && _stack.top().type == KEY) _stack.pop();
+  if (!_stack.empty() && _stack.top().type == ARR) valCheck();
+  if (_stack.size() && _stack.top().type == ARR) prettor();
+  (*_out) << "{";
+  _stack.push({OBJ, 1});
+}
+
+// _____________________________________________________________________________
+void JsonWriter::obj(const std::map<std::string, std::string> kvs) {
+  obj();
+  for (const auto& kv : kvs) {
+    key(kv.first);
+    val(kv.second);
+  }
+  close();
+}
+
+// _____________________________________________________________________________
+void JsonWriter::key(const std::string& k) {
+  if (_stack.empty() || _stack.top().type != OBJ)
+    throw JsonWriterException("Keys only allowed in objects.");
+  if (!_stack.top().empty) (*_out) << "," << (_pretty ? " " : "");
+  _stack.top().empty = 0;
+  prettor();
+  (*_out) << "\"" << k << "\""
+          << ":" << (_pretty ? " " : "");
+  _stack.push({KEY, 1});
+}
+
+// _____________________________________________________________________________
+void JsonWriter::keyVal(const std::string& k, const std::string& v) {
+  key(k);
+  val(v);
+}
+
+// _____________________________________________________________________________
+void JsonWriter::valCheck() {
+  if (_stack.empty() || (_stack.top().type != KEY && _stack.top().type != ARR))
+    throw JsonWriterException("Value not allowed here.");
+  if (!_stack.empty() && _stack.top().type == KEY) _stack.pop();
+  if (!_stack.empty() && _stack.top().type == ARR) {
+    if (!_stack.top().empty) (*_out) << "," << (_pretty ? " " : "");
+    _stack.top().empty = 0;
+  }
+}
+
+// _____________________________________________________________________________
+void JsonWriter::val(const std::string& v) {
+  valCheck();
+  (*_out) << "\"" << util::jsonStringEscape(v) << "\"";
+}
+
+// _____________________________________________________________________________
+void JsonWriter::val(int v) {
+  valCheck();
+  (*_out) << v;
+}
+
+// _____________________________________________________________________________
+void JsonWriter::val(double v) {
+  valCheck();
+  (*_out) << v;
+}
+
+// _____________________________________________________________________________
+void JsonWriter::arr() {
+  if (!_stack.empty() && _stack.top().type == OBJ)
+    throw JsonWriterException("Array not allowed as key");
+  if (!_stack.empty() && _stack.top().type == KEY) _stack.pop();
+  if (!_stack.empty() && _stack.top().type == ARR) valCheck();
+  (*_out) << "[";
+  _stack.push({ARR, 1});
+}
+
+// _____________________________________________________________________________
+void JsonWriter::prettor() {
+  if (_pretty) {
+    (*_out) << "\n";
+    for (size_t i = 0; i < _indent * _stack.size(); i++) (*_out) << " ";
+  }
+}
+
+// _____________________________________________________________________________
+void JsonWriter::closeAll() {
+  while (!_stack.empty()) close();
+}
+
+// _____________________________________________________________________________
+void JsonWriter::close() {
+  if (_stack.empty()) return;
+  switch (_stack.top().type) {
+    case OBJ:
+      _stack.pop();
+      prettor();
+      (*_out) << "}";
+      break;
+    case ARR:
+      _stack.pop();
+      (*_out) << "]";
+      break;
+    case KEY:
+      throw JsonWriterException("Missing value.");
+  }
+}

src/util/json/JsonWriter.h

@@ -0,0 +1,70 @@
+// Copyright 2018, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+#ifndef UTIL_JSON_JSONWRITER_H_
+#define UTIL_JSON_JSONWRITER_H_
+
+#include <map>
+#include <ostream>
+#include <stack>
+#include <string>
+
+namespace util {
+namespace json {
+
+class JsonWriterException : public std::exception {
+ public:
+  JsonWriterException(std::string msg) : _msg(msg) {}
+  ~JsonWriterException() throw() {}
+
+  virtual const char* what() const throw() { return _msg.c_str(); };
+
+ private:
+  std::string _msg;
+};
+
+// simple JSON writer class without much overhead
+class JsonWriter {
+ public:
+  explicit JsonWriter(std::ostream* out);
+  JsonWriter(std::ostream* out, size_t prec);
+  JsonWriter(std::ostream* out, size_t prec, bool pretty);
+  JsonWriter(std::ostream* out, size_t prec, bool pretty, size_t indent);
+  ~JsonWriter(){};
+
+  void obj();
+  void obj(const std::map<std::string, std::string> kvs);
+  void arr();
+  void key(const std::string& k);
+  void val(const std::string& v);
+  void val(int v);
+  void val(double v);
+  void keyVal(const std::string& k, const std::string& v);
+
+  void close();
+  void closeAll();
+
+ private:
+  std::ostream* _out;
+
+  enum JSON_NODE_T { OBJ, ARR, KEY };
+
+  struct JsonNode {
+    JSON_NODE_T type;
+    bool empty;
+  };
+
+  std::stack<JsonNode> _stack;
+
+  bool _pretty;
+  size_t _indent;
+
+  void valCheck();
+  void prettor();
+};
+
+}  // namespace json
+}  // namespace util
+
+#endif  // UTIL_JSON_JSONWRITER_H_

src/util/log/Log.h

@@ -0,0 +1,54 @@
+// Copyright 2017, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+#ifndef UTIL_LOG_LOG_H_
+#define UTIL_LOG_LOG_H_
+
+#include <chrono>
+#include <iomanip>
+#include <iostream>
+
+#define VDEBUG 4
+#define DEBUG 3
+#define INFO 2
+#define WARN 1
+#define ERROR 0
+
+#ifndef LOGLEVEL
+#define LOGLEVEL 2
+#endif
+
+// compiler will optimize statement away if x > LOGLEVEL
+#define LOG(x) if (x <= LOGLEVEL) util::Log<x>().log()
+
+using std::setfill;
+using std::setw;
+using namespace std::chrono;
+
+namespace util {
+
+static const char* LOGS[] = {"ERROR", "WARN ", "INFO ", "DEBUG", "DEBUG"};
+
+template <char LVL>
+class Log {
+ public:
+  Log() { if (LVL == ERROR) os = &std::cerr; else os = &std::cout; }
+  ~Log() { (*os) << std::endl; }
+  std::ostream& log() { return ts() << LOGS[(size_t)LVL] << ": "; }
+
+ private:
+  std::ostream* os;
+  std::ostream& ts() {
+    char tl[20];
+    auto n = system_clock::now();
+    time_t tt = system_clock::to_time_t(n);
+    int m = duration_cast<milliseconds>(n-time_point_cast<seconds>(n)).count();
+    struct tm t = *localtime(&tt);
+    strftime(tl, 20, "%Y-%m-%d %H:%M:%S", &t);
+    return (*os) << "[" << tl << "." << setfill('0') << setw(3) << m << "] ";
+  }
+};
+}
+
+#endif  // UTIL_LOG_LOG_H_

src/util/tests/CMakeLists.txt

@@ -0,0 +1,6 @@
+include_directories(
+	${TRANSITMAP_INCLUDE_DIR}
+)
+
+add_executable(utilTest TestMain.cpp)
+target_link_libraries(utilTest util)

src/util/tests/TestMain.cpp

@@ -0,0 +1,851 @@
+// Copyright 2016
+// Author: Patrick Brosi
+//
+
+#include <string>
+#include "lest.h"
+#include "util/Nullable.h"
+#include "util/String.h"
+#include "util/geo/Geo.h"
+#include "util/graph/DirGraph.h"
+#include "util/graph/UndirGraph.h"
+#include "util/graph/Dijkstra.h"
+#include "util/graph/EDijkstra.h"
+#include "util/geo/Grid.h"
+#include "util/Misc.h"
+
+using lest::approx;
+using namespace util;
+using namespace util::geo;
+using namespace util::graph;
+
+// define LEST cases
+const lest::test specification[] = {
+
+// ___________________________________________________________________________
+CASE("atof") {
+  EXPECT(util::atof("45.534215") == approx(45.534215));
+  EXPECT(util::atof("5.534") == approx(5.534));
+  EXPECT(util::atof("534") == approx(534));
+  EXPECT(util::atof("-534") == approx(-534));
+  EXPECT(util::atof("-45.534215") == approx(-45.534215));
+  EXPECT(util::atof("-45.534215", 2) == approx(-45.53));
+
+
+  // TODO: more test cases
+},
+
+// ___________________________________________________________________________
+CASE("dirgraph") {
+  DirGraph<int, int> g;
+
+  DirNode<int, int>* a = new DirNode<int, int>(0);
+  DirNode<int, int>* b = new DirNode<int, int>(0);
+  g.addNd(a);
+  EXPECT(g.getNds()->size() == (size_t)1);
+  g.addNd(b);
+  EXPECT(g.getNds()->size() == (size_t)2);
+
+  g.addEdg(a, b);
+  EXPECT(a->getDeg() == (size_t)1);
+  EXPECT(b->getDeg() == (size_t)0);
+
+  auto c = g.addNd();
+
+  g.addEdg(a, c);
+  g.addEdg(c, b);
+  EXPECT(a->getDeg() == (size_t)2);
+  EXPECT(b->getDeg() == (size_t)0);
+  EXPECT(c->getDeg() == (size_t)1);
+
+  g.delEdg(a, c);
+
+  EXPECT(a->getDeg() == (size_t)1);
+  EXPECT(b->getDeg() == (size_t)0);
+  EXPECT(c->getDeg() == (size_t)1);
+
+  g.addEdg(a, a);
+  EXPECT(a->getDeg() == (size_t)2);
+
+  g.delEdg(a, a);
+  EXPECT(a->getDeg() == (size_t)1);
+
+  g.delEdg(a, a);
+  EXPECT(a->getDeg() == (size_t)1);
+
+  // TODO: more test cases 
+},
+
+// ___________________________________________________________________________
+CASE("unddirgraph") {
+  UndirGraph<int, int> g;
+
+  UndirNode<int, int>* a = new UndirNode<int, int>(0);
+  UndirNode<int, int>* b = new UndirNode<int, int>(0);
+  g.addNd(a);
+  EXPECT(g.getNds()->size() == (size_t)1);
+  g.addNd(b);
+  EXPECT(g.getNds()->size() == (size_t)2);
+
+  g.addEdg(a, b);
+  EXPECT(a->getDeg() == (size_t)1);
+  EXPECT(b->getDeg() == (size_t)1);
+
+  auto c = g.addNd();
+
+  g.addEdg(a, c);
+  g.addEdg(c, b);
+  EXPECT(a->getDeg() == (size_t)2);
+  EXPECT(b->getDeg() == (size_t)2);
+  EXPECT(c->getDeg() == (size_t)2);
+
+  g.delEdg(a, c);
+
+  EXPECT(a->getDeg() == (size_t)1);
+  EXPECT(b->getDeg() == (size_t)2);
+  EXPECT(c->getDeg() == (size_t)1);
+
+  g.delNd(b);
+
+  EXPECT(a->getDeg() == (size_t)0);
+  EXPECT(c->getDeg() == (size_t)0);
+
+  g.addEdg(a, a);
+  EXPECT(a->getDeg() == (size_t)1);
+
+  g.delEdg(a, a);
+  EXPECT(a->getDeg() == (size_t)0);
+
+  // TODO: more test cases
+
+},
+
+// ___________________________________________________________________________
+CASE("grid") {
+  Grid<int, Line, double> g(.5, .5, Box<double>(Point<double>(0, 0), Point<double>(3, 3)));
+
+  Line<double> l;
+  l.push_back(Point<double>(0, 0));
+  l.push_back(Point<double>(1.5, 2));
+
+  Line<double> l2;
+  l.push_back(Point<double>(2.5, 1));
+  l.push_back(Point<double>(2.5, 2));
+
+  g.add(l, 1);
+  g.add(l2, 2);
+
+  std::set<int> ret;
+
+  Box<double> req(Point<double>(.5, 1), Point<double>(1, 1.5));
+  g.get(req, &ret);
+  EXPECT(ret.size() == (size_t)1);
+
+  ret.clear();
+  g.getNeighbors(1, 0, &ret);
+  // TODO!
+  //EXPECT(ret.size() == 1);
+
+  ret.clear();
+  g.getNeighbors(1, 0.55, &ret);
+  EXPECT(ret.size() == (size_t)2);
+
+
+  // TODO: more test cases
+},
+
+// ___________________________________________________________________________
+CASE("densify") {
+  Line<double> a;
+  a.push_back(Point<double>(1, 1));
+  a.push_back(Point<double>(10, 1));
+
+  auto dense = util::geo::densify(a, 1);
+
+  EXPECT(dense.size() == (size_t)10);
+
+  for (int i = 0; i < 10; i++) {
+    EXPECT(dense[i].get<0>() == approx(i + 1.0));
+  }
+
+  dense = util::geo::simplify(dense, 0.1);
+  EXPECT(dense.size() == (size_t)2);
+
+  Line<double> b;
+  b.push_back(Point<double>(1, 1));
+  b.push_back(Point<double>(5, 7));
+  b.push_back(Point<double>(10, 3));
+
+  dense = util::geo::densify(b, 1);
+
+  dense = util::geo::simplify(dense, 0.1);
+  EXPECT(dense.size() == (size_t)3);
+},
+
+// ___________________________________________________________________________
+CASE("summed frechet distance") {
+  Line<double> a;
+  a.push_back(Point<double>(1, 1));
+  a.push_back(Point<double>(2, 1));
+  a.push_back(Point<double>(3, 1));
+  a.push_back(Point<double>(3, 2));
+  a.push_back(Point<double>(4, 2));
+  a.push_back(Point<double>(4, 1));
+  a.push_back(Point<double>(5, 1));
+  a.push_back(Point<double>(6, 1));
+
+  Line<double> b;
+  b.push_back(Point<double>(1, 1));
+  b.push_back(Point<double>(2, 1));
+  b.push_back(Point<double>(3, 1));
+  b.push_back(Point<double>(4, 1));
+  b.push_back(Point<double>(5, 1));
+  b.push_back(Point<double>(6, 1));
+
+  double fd = util::geo::accFrechetDistC(a, b, 0.1);
+  EXPECT(fd == approx(2));
+},
+
+// ___________________________________________________________________________
+CASE("frechet distance") {
+  Line<double> e;
+  e.push_back(Point<double>(1, 1));
+  e.push_back(Point<double>(1, 2));
+
+  Line<double> f;
+  f.push_back(Point<double>(1, 1));
+  f.push_back(Point<double>(1, 2));
+
+  double fd = util::geo::frechetDist(e, f, 0.1);
+
+  EXPECT(fd == approx(0));
+
+  Line<double> a;
+  a.push_back(Point<double>(1, 1));
+  a.push_back(Point<double>(2, 1));
+  a.push_back(Point<double>(3, 2));
+  a.push_back(Point<double>(4, 2));
+  a.push_back(Point<double>(5, 1));
+  a.push_back(Point<double>(6, 1));
+
+  Line<double> b;
+  b.push_back(Point<double>(1, 1));
+  b.push_back(Point<double>(2, 1));
+  b.push_back(Point<double>(3, 1));
+  b.push_back(Point<double>(4, 1));
+  b.push_back(Point<double>(5, 1));
+  b.push_back(Point<double>(6, 1));
+
+  auto adense = util::geo::densify(a, 0.1);
+  auto bdense = util::geo::densify(b, 0.1);
+
+  fd = util::geo::frechetDist(a, b, 0.1);
+
+  EXPECT(fd == approx(1));
+
+  Line<double> c;
+  c.push_back(Point<double>(1, 1));
+  c.push_back(Point<double>(2, 1));
+
+  Line<double> d;
+  d.push_back(Point<double>(3, 1));
+  d.push_back(Point<double>(4, 1));
+
+  fd = util::geo::frechetDist(c, d, 0.1);
+
+  EXPECT(fd == approx(2));
+
+
+  Line<double> g;
+  g.push_back(Point<double>(1, 1));
+  g.push_back(Point<double>(10, 1));
+
+  Line<double> h;
+  h.push_back(Point<double>(1, 1));
+  h.push_back(Point<double>(3, 2));
+  h.push_back(Point<double>(3, 1));
+  h.push_back(Point<double>(10, 1));
+
+  fd = util::geo::frechetDist(g, h, 0.1);
+
+  EXPECT(fd == approx(1));
+},
+
+// ___________________________________________________________________________
+CASE("geo box alignment") {
+  Line<double> a;
+  a.push_back(Point<double>(1, 1));
+  a.push_back(Point<double>(1, 2));
+
+  Line<double> b;
+  b.push_back(Point<double>(1, 2));
+  b.push_back(Point<double>(2, 2));
+
+  Line<double> c;
+  c.push_back(Point<double>(2, 2));
+  c.push_back(Point<double>(2, 1));
+
+  Line<double> d;
+  d.push_back(Point<double>(2, 1));
+  d.push_back(Point<double>(1, 1));
+
+  Box<double> box(Point<double>(2, 3), Point<double>(5, 4));
+  MultiLine<double> ml;
+  ml.push_back(a);
+  ml.push_back(b);
+  ml.push_back(c);
+  ml.push_back(d);
+
+  EXPECT(parallelity(box, ml) == approx(1));
+  ml = rotate(ml, 45);
+  EXPECT(parallelity(box, ml) == approx(0));
+  ml = rotate(ml, 45);
+  EXPECT(parallelity(box, ml) == approx(1));
+  ml = rotate(ml, 45);
+  EXPECT(parallelity(box, ml) == approx(0));
+  ml = rotate(ml, 45);
+  EXPECT(parallelity(box, ml) == approx(1));
+},
+
+// ___________________________________________________________________________
+CASE("url decode") {
+  EXPECT("zürich" == util::urlDecode("z%C3%BCrich"));
+  EXPECT("!@$%^*()" == util::urlDecode("!%40%24%25%5E*()"));
+  EXPECT("Løkken" == util::urlDecode("L%C3%B8kken"));
+  EXPECT("á é" == util::urlDecode("%C3%A1%20%C3%A9"));
+  EXPECT("á é" == util::urlDecode("%C3%A1+%C3%A9"));
+},
+
+// ___________________________________________________________________________
+CASE("json escape") {
+  EXPECT("Hello\\\\Goodbye!" == util::jsonStringEscape("Hello\\Goodbye!"));
+  EXPECT("\\\"Hello\\\"" == util::jsonStringEscape("\"Hello\""));
+},
+
+// ___________________________________________________________________________
+CASE("split") {
+  EXPECT(util::split("hello,again", ',').size() == (size_t)2);
+  EXPECT(util::split("hello,,again", ',').size() == (size_t)3);
+  EXPECT(util::split("hello", ',').size() == (size_t)1);
+  EXPECT(util::split("", ',').size() == (size_t)0);
+},
+
+// ___________________________________________________________________________
+CASE("editdist") {
+  EXPECT(util::editDist("hello", "mello") == (size_t)1);
+  EXPECT(util::editDist("mello", "hello") == (size_t)1);
+  EXPECT(util::editDist("abcde", "abfde") == (size_t)1);
+  EXPECT(util::editDist("abcd", "abcde") == (size_t)1);
+  EXPECT(util::editDist("xabcd", "abcde") == (size_t)2);
+  EXPECT(util::editDist("abcd", "abcdes") == (size_t)2);
+  EXPECT(util::editDist("hello", "hello") == (size_t)0);
+},
+
+// ___________________________________________________________________________
+CASE("toString") {
+  EXPECT(util::toString(34) == "34");
+  EXPECT(util::toString("34") == "34");
+},
+
+// ___________________________________________________________________________
+CASE("replace") {
+  std::string a("lorem ipsum ipsum lorem");
+
+  EXPECT(util::replace(a, "ips", "aa"));
+  EXPECT(a == "lorem aaum ipsum lorem");
+
+  EXPECT(!util::replace(a, "blablabla", ""));
+  EXPECT(a == "lorem aaum ipsum lorem");
+
+  EXPECT(util::replace(a, "m", ""));
+  EXPECT(a == "lore aaum ipsum lorem");
+
+  EXPECT(!util::replace(a, "", ""));
+  EXPECT(a == "lore aaum ipsum lorem");
+
+  std::string b("lorem ipsum ipsum lorem");
+  EXPECT(util::replaceAll(b, "ips", "aa"));
+  EXPECT(b == "lorem aaum aaum lorem");
+
+  EXPECT(util::replaceAll(b, "m", ""));
+  EXPECT(b == "lore aau aau lore");
+
+  EXPECT(util::replaceAll(b, "a", "aa"));
+  EXPECT(b == "lore aaaau aaaau lore");
+
+  EXPECT(util::replaceAll(b, "e", "e"));
+  EXPECT(b == "lore aaaau aaaau lore");
+
+  EXPECT(util::replaceAll(b, "e", "ee"));
+  EXPECT(b == "loree aaaau aaaau loree");
+
+  EXPECT(!util::replaceAll(b, "", "ee"));
+  EXPECT(b == "loree aaaau aaaau loree");
+},
+
+// ___________________________________________________________________________
+CASE("Edge-based Dijkstra directed, 1 to all") {
+  DirGraph<std::string, int> g;
+
+  auto a = g.addNd("A");
+  auto b = g.addNd("B");
+  auto c = g.addNd("C");
+  auto d = g.addNd("D");
+  auto e = g.addNd("E");
+
+  auto eAC = g.addEdg(a, c, 1);
+  auto eAB = g.addEdg(a, b, 5);
+  auto eDC = g.addEdg(d, c, 1);
+  auto eDB = g.addEdg(d, b, 3);
+  auto eED = g.addEdg(e, d, 1);
+  auto eEB = g.addEdg(e, b, 1);
+
+
+  UNUSED(eAC);
+  UNUSED(eDC);
+  UNUSED(eDB);
+  UNUSED(eED);
+  UNUSED(eEB);
+
+  struct CostFunc : public EDijkstra::CostFunc<std::string, int, int> {
+    int operator()(const Edge<std::string, int>* from,
+                   const Node<std::string, int>* n,
+                   const Edge<std::string, int>* to) const {
+      UNUSED(from);
+
+      // dont count cost of start edge
+      if (n) return to->pl();
+      return 0;
+    };
+    int inf() const { return 999; };
+  };
+
+  CostFunc cFunc;
+
+  auto cost = EDijkstra::shortestPath(eAB, cFunc);
+
+  for (auto u : cost) {
+    int single = EDijkstra::shortestPath(eAB, u.first, cFunc);
+    EXPECT(single == u.second);
+  }
+
+  // all to 1
+  auto eBC = g.addEdg(b, c, 10);
+
+  auto costb = EDijkstra::shortestPathRev(eBC, cFunc);
+  for (auto u : costb) {
+    int single = EDijkstra::shortestPath(u.first, eBC, cFunc);
+    EXPECT(single == u.second);
+  }
+},
+
+// ___________________________________________________________________________
+CASE("Edge-based Dijkstra undirected, edge 1 to 1") {
+  UndirGraph<std::string, int> g;
+
+  auto a = g.addNd("A");
+  auto b = g.addNd("B");
+  auto c = g.addNd("C");
+  auto d = g.addNd("D");
+  auto e = g.addNd("E");
+
+  auto eAC = g.addEdg(a, c, 1);
+  auto eAB = g.addEdg(a, b, 5);
+  auto eDC = g.addEdg(d, c, 1);
+  auto eDB = g.addEdg(d, b, 3);
+  auto eED = g.addEdg(e, d, 1);
+  auto eEB = g.addEdg(e, b, 1);
+
+  UNUSED(eAC);
+  UNUSED(eDC);
+  UNUSED(eDB);
+  UNUSED(eED);
+  UNUSED(eEB);
+
+  struct CostFunc : public EDijkstra::CostFunc<std::string, int, int> {
+    int operator()(const Edge<std::string, int>* from,
+                   const Node<std::string, int>* n,
+                   const Edge<std::string, int>* to) const {
+      UNUSED(from);
+
+      // dont count cost of start edge
+      if (n) return to->pl();
+      return 0;
+    };
+    int inf() const { return 999; };
+  };
+
+  CostFunc cFunc;
+
+  EDijkstra::NList<std::string, int> res;
+  EDijkstra::EList<std::string, int> resE;
+  int cost = EDijkstra::shortestPath(eAB, d, cFunc, &resE, &res);
+
+  EXPECT(cost == 2);
+
+  EXPECT(resE.size() == (size_t)3);
+  EXPECT(res.size() == (size_t)3);
+  EXPECT((*(res.rbegin()))->pl() == "A");
+  EXPECT((*(++res.rbegin()))->pl() == "C");
+  EXPECT((*(++++res.rbegin()))->pl() == "D");
+
+  EXPECT((*(resE.rbegin())) == eAB);
+  EXPECT((*(++resE.rbegin())) == eAC);
+  EXPECT((*(++++resE.rbegin())) == eDC);
+
+  cost = EDijkstra::shortestPath(eAB, b, cFunc, &resE, &res);
+  EXPECT(cost == 0);
+},
+
+// ___________________________________________________________________________
+CASE("Edge-based Dijkstra undirected, edge 1 to n") {
+  UndirGraph<std::string, int> g;
+
+  auto a = g.addNd("A");
+  auto b = g.addNd("B");
+  auto c = g.addNd("C");
+  auto d = g.addNd("D");
+  auto e = g.addNd("E");
+
+  auto eAC = g.addEdg(a, c, 1);
+  auto eAB = g.addEdg(a, b, 5);
+  auto eDC = g.addEdg(d, c, 1);
+  auto eDB = g.addEdg(d, b, 3);
+  auto eED = g.addEdg(e, d, 1);
+  auto eEB = g.addEdg(e, b, 1);
+
+  UNUSED(eAC);
+  UNUSED(eDC);
+  UNUSED(eDB);
+  UNUSED(eED);
+  UNUSED(eEB);
+
+  struct CostFunc : public EDijkstra::CostFunc<std::string, int, int> {
+    int operator()(const Edge<std::string, int>* from, const Node<std::string, int>* n,
+                   const Edge<std::string, int>* to) const {
+      UNUSED(from);
+
+      // dont count cost of start edge
+      if (n) return to->pl();
+      return 0;
+    };
+    int inf() const { return 999; };
+  };
+
+  CostFunc cFunc;
+
+  std::set<Node<std::string, int>*> tos;
+  tos.insert(d);
+  tos.insert(b);
+  tos.insert(b);
+
+  EDijkstra::NList<std::string, int> res;
+  EDijkstra::EList<std::string, int> resE;
+  int cost = EDijkstra::shortestPath(eAB, tos, cFunc, &resE, &res);
+  EXPECT(cost == 0);
+},
+
+// ___________________________________________________________________________
+CASE("Edge-based Dijkstra undirected, 1 to n") {
+  UndirGraph<std::string, int> g;
+
+  auto a = g.addNd("A");
+  auto b = g.addNd("B");
+  auto c = g.addNd("C");
+  auto d = g.addNd("D");
+  auto e = g.addNd("E");
+
+  g.addEdg(a, c, 1);
+  auto eAB = g.addEdg(a, b, 5);
+  auto eDC = g.addEdg(d, c, 1);
+  g.addEdg(d, b, 3);
+  auto eED = g.addEdg(e, d, 1);
+  g.addEdg(e, b, 1);
+
+  struct CostFunc : public EDijkstra::CostFunc<std::string, int, int> {
+    int operator()(const Edge<std::string, int>* from, const Node<std::string, int>* n,
+                   const Edge<std::string, int>* to) const {
+      UNUSED(from);
+
+      // dont count cost of start edge
+      if (n) return to->pl();
+      return 0;
+    };
+    int inf() const { return 999; };
+  };
+
+  CostFunc cFunc;
+
+  std::set<Edge<std::string, int>*> tos;
+  tos.insert(eDC);
+  tos.insert(eED);
+
+  std::unordered_map<Edge<std::string, int>*, EDijkstra::EList<std::string, int>*> resE;
+  resE[eDC] = new EDijkstra::EList<std::string, int>();
+  resE[eED] = new EDijkstra::EList<std::string, int>();
+  std::unordered_map<Edge<std::string, int>*, EDijkstra::NList<std::string, int>*> res;
+  res[eDC] = new EDijkstra::NList<std::string, int>();
+  res[eED] = new EDijkstra::NList<std::string, int>();
+  auto hFunc = EDijkstra::ZeroHeurFunc<std::string, int, int>();
+  std::unordered_map<Edge<std::string, int>*, int> cost = EDijkstra::shortestPath(eAB, tos, cFunc, hFunc, resE, res);
+
+  EXPECT(cost[eDC] == 2);
+  EXPECT(cost[eED] == 2);
+
+  EXPECT(resE[eDC]->size() == (size_t)3);
+  EXPECT(res[eED]->size() == (size_t)3);
+
+  EXPECT(resE[eDC]->size() == (size_t)3);
+  EXPECT(res[eED]->size() == (size_t)3);
+},
+
+// ___________________________________________________________________________
+CASE("Edge-based Dijkstra undirected") {
+  UndirGraph<std::string, int> g;
+
+  auto a = g.addNd("A");
+  auto b = g.addNd("B");
+  auto c = g.addNd("C");
+  auto d = g.addNd("D");
+  auto e = g.addNd("E");
+
+  g.addEdg(a, c, 1);
+  g.addEdg(a, b, 5);
+  g.addEdg(d, c, 1);
+  g.addEdg(d, b, 3);
+  g.addEdg(e, d, 1);
+  g.addEdg(e, b, 1);
+
+  struct CostFunc : public EDijkstra::CostFunc<std::string, int, int> {
+    int operator()(const Edge<std::string, int>* fr, const Node<std::string, int>* n,
+                   const Edge<std::string, int>* to) const {
+      UNUSED(fr);
+      UNUSED(n);
+      return to->pl();
+    };
+    int inf() const { return 999; };
+  };
+
+  CostFunc cFunc;
+
+  EDijkstra::NList<std::string, int> res;
+  EDijkstra::EList<std::string, int> resE;
+  int cost = EDijkstra::shortestPath(a, b, cFunc, &resE, &res);
+
+  EXPECT(res.size() == (size_t)5);
+  EXPECT((*(res.rbegin()))->pl() == "A");
+  EXPECT((*(++res.rbegin()))->pl() == "C");
+  EXPECT((*(++++res.rbegin()))->pl() == "D");
+  EXPECT((*(++++++res.rbegin()))->pl() == "E");
+  EXPECT((*(++++++++res.rbegin()))->pl() == "B");
+  EXPECT(cost == 4);
+  EXPECT((*(resE.rbegin()))->getFrom()->pl() == "A");
+  EXPECT((*(++resE.rbegin()))->getFrom()->pl() == "D");
+  EXPECT((*(++++resE.rbegin()))->getFrom()->pl() == "E");
+  EXPECT((*(++++++resE.rbegin()))->getTo()->pl() == "B");
+
+  EXPECT(resE.size() == (size_t)4);
+
+  cost = EDijkstra::shortestPath(d, b, cFunc, &res);
+  EXPECT(cost == 2);
+
+  cost = EDijkstra::shortestPath(b, d, cFunc, &res);
+  EXPECT(cost == 2);
+
+  cost = EDijkstra::shortestPath(e, b, cFunc, &res);
+  EXPECT(cost == 1);
+
+  cost = EDijkstra::shortestPath(b, e, cFunc, &res);
+  EXPECT(cost == 1);
+
+  cost = EDijkstra::shortestPath(b, a, cFunc, &res);
+  EXPECT(cost == 4);
+
+  cost = EDijkstra::shortestPath(c, a, cFunc, &res);
+  EXPECT(cost == 1);
+
+  cost = EDijkstra::shortestPath(a, c, cFunc, &res);
+  EXPECT(cost == 1);
+
+  cost = EDijkstra::shortestPath(a, d, cFunc, &res);
+  EXPECT(cost == 2);
+},
+
+// ___________________________________________________________________________
+CASE("Edge-based Dijkstra") {
+  DirGraph<int, int> g;
+
+  DirNode<int, int>* a = new DirNode<int, int>(1);
+  DirNode<int, int>* b = new DirNode<int, int>(4);
+  g.addNd(a);
+  g.addNd(b);
+
+  auto c = g.addNd(2);
+  auto d = g.addNd(3);
+  auto x = g.addNd();
+
+  g.addEdg(a, d, 4);
+  g.addEdg(a, c, 1);
+  g.addEdg(c, b, 1);
+  g.addEdg(b, d, 1);
+
+  struct CostFunc : public EDijkstra::CostFunc<int, int, int> {
+    int operator()(const Edge<int, int>* fr, const Node<int, int>* n,
+                   const Edge<int, int>* to) const {
+      UNUSED(fr);
+      UNUSED(n);
+      return to->pl();
+    };
+    int inf() const { return 999; };
+  };
+
+  CostFunc cFunc;
+
+  EDijkstra::NList<int, int> res;
+  int cost = EDijkstra::shortestPath(a, d, cFunc, &res);
+
+  EXPECT(cost == 3);
+
+  g.addEdg(c, d, 3);
+  cost = EDijkstra::shortestPath(a, d, cFunc, &res);
+
+  EXPECT(cost == 3);
+
+  g.addEdg(a, b, 1);
+  g.addEdg(x, a, 1);
+  cost = EDijkstra::shortestPath(a, d, cFunc, &res);
+
+  EXPECT(cost == 2);
+},
+
+// ___________________________________________________________________________
+CASE("Dijkstra") {
+  DirGraph<int, int> g;
+
+  DirNode<int, int>* a = new DirNode<int, int>(1);
+  DirNode<int, int>* b = new DirNode<int, int>(0);
+  g.addNd(a);
+  g.addNd(b);
+
+  auto c = g.addNd();
+  auto d = g.addNd(4);
+  auto x = g.addNd();
+
+  g.addEdg(a, d, 4);
+  g.addEdg(a, c, 1);
+  g.addEdg(c, b, 1);
+  g.addEdg(b, d, 1);
+
+  struct CostFunc : public Dijkstra::CostFunc<int, int, int> {
+    int operator()(const Node<int, int>* fr, const Edge<int, int>* e,
+                   const Node<int, int>* to) const {
+      UNUSED(fr);
+      UNUSED(to);
+      return e->pl();
+    };
+    int inf() const { return 999; };
+  };
+
+  CostFunc cFunc;
+
+  Dijkstra::NList<int, int> res;
+  int cost = Dijkstra::shortestPath(a, d, cFunc, &res);
+
+  EXPECT(cost == 3);
+  EXPECT(res.size() == (size_t)4);
+
+  g.addEdg(c, d, 3);
+  cost = Dijkstra::shortestPath(a, d, cFunc, &res);
+
+  EXPECT(cost == 3);
+
+  g.addEdg(a, b, 1);
+  g.addEdg(x, a, 1);
+  cost = Dijkstra::shortestPath(a, d, cFunc, &res);
+
+  EXPECT(cost == 2);
+
+  const std::set<Node<int, int>*> to{b, c, d, x};
+  std::unordered_map<Node<int, int>*, Dijkstra::EList<int, int>*> resEdges;
+  std::unordered_map<Node<int, int>*, Dijkstra::NList<int, int>*> resNodes;
+
+  for (auto n : to) {
+    resEdges[n] = new Dijkstra::EList<int, int>();
+    resNodes[n] = new Dijkstra::NList<int, int>();
+  }
+
+  auto costs = Dijkstra::shortestPath(a, to, cFunc, resEdges, resNodes);
+
+  EXPECT(costs[b] == 1);
+  EXPECT(costs[c] == 1);
+  EXPECT(costs[d] == 2);
+  EXPECT(costs[x] == 999);
+},
+
+// ___________________________________________________________________________
+CASE("nullable") {
+  {
+    util::Nullable<std::string> nullable;
+    EXPECT(nullable.isNull());
+  }
+
+  {
+    util::Nullable<std::string> nullable(0);
+    EXPECT(nullable.isNull());
+  }
+
+  {
+    std::string str = "aa";
+    util::Nullable<std::string> nullable(&str);
+    EXPECT(!nullable.isNull());
+
+    EXPECT(nullable == "aa");
+    EXPECT(!(nullable == "aaa"));
+    EXPECT(!(nullable != "aa"));
+    EXPECT(nullable == "aa");
+
+    EXPECT(nullable.get() == "aa");
+    EXPECT(std::string(nullable) == "aa");
+  }
+
+  {
+    int a = 23;
+    util::Nullable<int> nullable(a);
+    util::Nullable<int> nullable2(24);
+    EXPECT(!nullable.isNull());
+
+    EXPECT(nullable == 23);
+    EXPECT(nullable >= 23);
+    EXPECT(nullable <= 23);
+    EXPECT(nullable < 24);
+    EXPECT(nullable < 24);
+    EXPECT(!(nullable < 22));
+    EXPECT(nullable != nullable2);
+    EXPECT(nullable < nullable2);
+    EXPECT(nullable2 > nullable);
+
+    util::Nullable<int> nullable3(nullable);
+    EXPECT(nullable == nullable3);
+
+    nullable3 = nullable2;
+    EXPECT(nullable2 == nullable3);
+    EXPECT(nullable3 == 24);
+    EXPECT(nullable2 == 24);
+    EXPECT(nullable2 == nullable2.get());
+    EXPECT(int(nullable2) == nullable2.get());
+    EXPECT(!nullable3.isNull());
+    EXPECT(!nullable2.isNull());
+
+    util::Nullable<int> voidnull;
+    EXPECT(voidnull.isNull());
+
+    EXPECT_THROWS(nullable == voidnull);
+  }
+}
+
+};
+
+// _____________________________________________________________________________
+int main(int argc, char** argv) {
+  return(lest::run(specification, argc, argv));
+}

src/util/xml/XmlWriter.cpp

@@ -0,0 +1,182 @@
+// Copyright 2016, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+#include <algorithm>
+#include <map>
+#include <ostream>
+#include <stack>
+#include <string>
+#include "XmlWriter.h"
+
+using namespace util;
+using namespace xml;
+
+using std::ostream;
+using std::string;
+using std::map;
+
+// _____________________________________________________________________________
+XmlWriter::XmlWriter(std::ostream* out)
+    : _out(out), _pretty(false), _indent(4) {}
+
+// _____________________________________________________________________________
+XmlWriter::XmlWriter(std::ostream* out, bool pret)
+    : _out(out), _pretty(pret), _indent(4) {}
+
+// _____________________________________________________________________________
+XmlWriter::XmlWriter(std::ostream* out, bool pret, size_t indent)
+    : _out(out), _pretty(pret), _indent(indent) {}
+
+// _____________________________________________________________________________
+void XmlWriter::openTag(const string& tag, const map<string, string>& attrs) {
+  if (!_nstack.empty() && _nstack.top().t == COMMENT) {
+    throw XmlWriterException("Opening tags not allowed while inside comment.");
+  }
+
+  checkTagName(tag);
+  closeHanging();
+  doIndent();
+
+  *_out << "<" << tag;
+
+  for (auto kv : attrs) {
+    *_out << " ";
+    putEsced(_out, kv.first, '"');
+    *_out << "=\"";
+    putEsced(_out, kv.second, '"');
+    *_out << "\"";
+  }
+
+  _nstack.push(XmlNode(TAG, tag, true));
+}
+
+// _____________________________________________________________________________
+void XmlWriter::openTag(const string& tag) {
+  openTag(tag, map<string, string>());
+}
+
+// _____________________________________________________________________________
+void XmlWriter::openTag(const string& tag, const string& k, const string& v) {
+  map<string, string> kv;
+  kv[k] = v;
+  openTag(tag, kv);
+}
+
+// _____________________________________________________________________________
+void XmlWriter::openComment() {
+  // don't allow nested comments
+  if (!_nstack.empty() && _nstack.top().t == COMMENT) return;
+
+  closeHanging();
+  doIndent();
+
+  *_out << "<!-- ";
+
+  _nstack.push(XmlNode(COMMENT, "", false));
+}
+
+// _____________________________________________________________________________
+void XmlWriter::writeText(const string& text) {
+  if (_nstack.empty()) {
+    throw XmlWriterException("Text content not allowed in prolog / trailing.");
+  }
+  closeHanging();
+  doIndent();
+  putEsced(_out, text, ' ');
+}
+
+// _____________________________________________________________________________
+void XmlWriter::closeTag() {
+  while (!_nstack.empty() && _nstack.top().t == TEXT) _nstack.pop();
+
+  if (_nstack.empty()) return;
+
+  if (_nstack.top().t == COMMENT) {
+    _nstack.pop();
+    doIndent();
+    *_out << " -->";
+  } else if (_nstack.top().t == TAG) {
+    if (_nstack.top().hanging) {
+      *_out << " />";
+      _nstack.pop();
+    } else {
+      string tag = _nstack.top().pload;
+      _nstack.pop();
+      doIndent();
+      *_out << "</" << tag << ">";
+    }
+  }
+}
+
+// _____________________________________________________________________________
+void XmlWriter::closeTags() {
+  while (!_nstack.empty()) closeTag();
+}
+
+// _____________________________________________________________________________
+void XmlWriter::doIndent() {
+  if (_pretty) {
+    *_out << std::endl;
+    for (size_t i = 0; i < _nstack.size() * _indent; i++) *_out << " ";
+  }
+}
+
+// _____________________________________________________________________________
+void XmlWriter::closeHanging() {
+  if (_nstack.empty()) return;
+
+  if (_nstack.top().hanging) {
+    *_out << ">";
+    _nstack.top().hanging = false;
+  } else if (_nstack.top().t == TEXT) {
+    _nstack.pop();
+  }
+}
+
+// _____________________________________________________________________________
+void XmlWriter::putEsced(ostream* out, const string& str, char quot) {
+  if (!_nstack.empty() && _nstack.top().t == COMMENT) {
+    *out << str;
+    return;
+  }
+
+  for (const char& c : str) {
+    if (quot == '"' && c == '"')
+      *out << "&quot;";
+    else if (quot == '\'' && c == '\'')
+      *out << "&apos;";
+    else if (c == '<')
+      *out << "&lt;";
+    else if (c == '>')
+      *out << "&gt;";
+    else if (c == '&')
+      *out << "&amp;";
+    else
+      *out << c;
+  }
+}
+
+// _____________________________________________________________________________
+void XmlWriter::checkTagName(const string& str) const {
+  if (!isalpha(str[0]) && str[0] != '_')
+    throw XmlWriterException(
+        "XML elements must start with either a letter "
+        "or an underscore");
+
+  string begin = str.substr(0, 3);
+  std::transform(begin.begin(), begin.end(), begin.begin(), ::tolower);
+  if (begin == "xml")
+    throw XmlWriterException(
+        "XML elements cannot start with"
+        " XML, xml, Xml etc.");
+
+  for (const char& c : str) {
+    // we allow colons in tag names for primitive namespace support
+    if (!isalpha(c) && !isdigit(c) && c != '-' && c != '_' && c != '.' &&
+        c != ':')
+      throw XmlWriterException(
+          "XML elements can only contain letters, "
+          "digits, hyphens, underscores and periods.");
+  }
+}

src/util/xml/XmlWriter.h

@@ -0,0 +1,91 @@
+// Copyright 2016, University of Freiburg,
+// Chair of Algorithms and Data Structures.
+// Authors: Patrick Brosi <brosi@informatik.uni-freiburg.de>
+
+#ifndef UTIL_XML_XMLWRITER_H_
+#define UTIL_XML_XMLWRITER_H_
+
+#include <map>
+#include <ostream>
+#include <stack>
+#include <string>
+
+namespace util {
+namespace xml {
+
+class XmlWriterException : public std::exception {
+ public:
+  XmlWriterException(std::string msg) : _msg(msg) {}
+  ~XmlWriterException() throw() {}
+
+  virtual const char* what() const throw() { return _msg.c_str(); };
+
+ private:
+  std::string _msg;
+};
+
+// simple XML writer class without much overhead
+class XmlWriter {
+ public:
+  explicit XmlWriter(std::ostream* out);
+  XmlWriter(std::ostream* out, bool pretty);
+  XmlWriter(std::ostream* out, bool pretty, size_t indent);
+  ~XmlWriter(){};
+
+  // open tag without attributes
+  void openTag(const std::string& tag);
+
+  // open tag with single attribute (for convenience...)
+  void openTag(const std::string& tag, const std::string& key,
+               const std::string& val);
+
+  // open tag with attribute list
+  void openTag(const std::string& tag,
+               const std::map<std::string, std::string>& attrs);
+
+  // open comment
+  void openComment();
+
+  // write text
+  void writeText(const std::string& text);
+
+  // close tag
+  void closeTag();
+
+  // close all open tags, essentially closing the document
+  void closeTags();
+
+ private:
+  enum XML_NODE_T { TAG, TEXT, COMMENT };
+
+  struct XmlNode {
+    XmlNode(XML_NODE_T t, const std::string& pload, bool hanging)
+        : t(t), pload(pload), hanging(hanging) {}
+    XML_NODE_T t;
+    std::string pload;
+    bool hanging;
+  };
+
+  std::ostream* _out;
+  std::stack<XmlNode> _nstack;
+
+  bool _pretty;
+  size_t _indent;
+
+  // handles indentation
+  void doIndent();
+
+  // close "hanging" tags
+  void closeHanging();
+
+  // pushes XML escaped text to stream
+  void putEsced(std::ostream* out, const std::string& str, char quot);
+
+  // checks tag names for validiy
+  void checkTagName(const std::string& str) const;
+};
+
+}  // namespace xml
+}  // namespace util
+
+#endif  // UTIL_XML_XMLWRITER_H_