diff --git a/src/util/geo/Box.h b/src/util/geo/Box.h
index 3c41e80..dfaae15 100644
--- a/src/util/geo/Box.h
+++ b/src/util/geo/Box.h
@@ -43,8 +43,8 @@ class RotatedBox {
  public:
   RotatedBox() : _box() {}
   RotatedBox(const Box<T>& box) : _box(box), _deg(0), _center() {}
-  RotatedBox(const Point<T>& ll, const Point<T>& ur) : _box(ll, ur), _deg(0), _center() {}
-
+  RotatedBox(const Point<T>& ll, const Point<T>& ur)
+      : _box(ll, ur), _deg(0), _center() {}
 
  private:
   Box<T> _box;
diff --git a/src/util/geo/Geo.h b/src/util/geo/Geo.h
index 4787c90..b176592 100644
--- a/src/util/geo/Geo.h
+++ b/src/util/geo/Geo.h
@@ -1,6 +1,7 @@
 // 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_
 
@@ -45,52 +46,12 @@ typedef Polygon<float> FPolygon;
 typedef Polygon<int> IPolygon;
 
 const static double EPSILON = 0.0000001;
-
-// _____________________________________________________________________________
-// 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.getX(), -center.getY());
-// bgeo::strategy::transform::rotate_transformer<bgeo::degree, T, 2, 2> rotate(
-// deg);
-// bgeo::strategy::transform::translate_transformer<T, 2, 2> translateBack(
-// center.getX(), center.getY());
-
-// 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;
-// }
+const static double RAD = 0.017453292519943295;  // PI/180
 
 // _____________________________________________________________________________
 // 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.getX(), -center.getY());
-// bgeo::strategy::transform::rotate_transformer<bgeo::degree, T, 2, 2> rotate(
-// deg);
-// bgeo::strategy::transform::translate_transformer<T, 2, 2> translateBack(
-// center.getX(), center.getY());
-
-// 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;
-// }
+// const Point<T>& center) {}
 
 // _____________________________________________________________________________
 template <typename T>
@@ -101,6 +62,42 @@ inline Box<T> pad(const Box<T>& box, double padding) {
                          box.getUpperRight().getY() + padding));
 }
 
+// _____________________________________________________________________________
+template <typename T>
+inline Point<T> centroid(const Point<T> p) {
+  return p;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline Point<T> centroid(const LineSegment<T> ls) {
+  return Point<T>((ls.first.getX() + ls.second.getX()) / T(2),
+                  (ls.first.getY() + ls.second.getY()) / T(2));
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline Point<T> centroid(const Line<T> ls) {
+  double x = 0, y = 0;
+  for (const auto& p : ls) {
+    x += p.getX();
+    y += p.getY();
+  }
+  return Point<T>(x / T(ls.size()), y / T(ls.size()));
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline Point<T> centroid(const Polygon<T> ls) {
+  return centroid(ls.getOuter());
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline Point<T> centroid(const Box<T> box) {
+  return centroid(LineSegment<T>(box.getLowerLeft(), box.getUpperRight()));
+}
+
 // _____________________________________________________________________________
 template <typename T>
 inline Point<T> rotate(const Point<T>& p, double deg) {
@@ -108,12 +105,47 @@ inline Point<T> rotate(const Point<T>& p, double deg) {
 }
 
 // _____________________________________________________________________________
-// 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 Point<T> rotate(Point<T> p, double deg, const Point<T>& c) {
+  deg *= -RAD;
+  double si = sin(deg);
+  double co = cos(deg);
+  p = p - c;
+
+  return Point<T>(p.getX() * co - p.getY() * si,
+                  p.getX() * si + p.getY() * co) +
+         c;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline LineSegment<T> rotate(LineSegment<T> geo, double deg,
+                             const Point<T>& c) {
+  geo.first = rotate(geo.first, deg, c);
+  geo.second = rotate(geo.second, deg, c);
+  return geo;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline LineSegment<T> rotate(LineSegment<T> geo, double deg) {
+  return (geo, deg, centroid(geo));
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline Line<T> rotate(Line<T> geo, double deg, const Point<T>& c) {
+  for (size_t i = 0; i < geo.size(); i++) geo[i] = rotate(geo[i], deg, c);
+  return geo;
+}
+
+// _____________________________________________________________________________
+template <typename T>
+inline Polygon<T> rotate(Polygon<T> geo, double deg, const Point<T>& c) {
+  for (size_t i = 0; i < geo.getOuter().size(); i++)
+    geo.getOuter()[i] = rotate(geo.getOuter()[i], deg, c);
+  return geo;
+}
 
 // _____________________________________________________________________________
 // template <typename T>
diff --git a/src/util/geo/Point.h b/src/util/geo/Point.h
index 942a361..df2b9aa 100644
--- a/src/util/geo/Point.h
+++ b/src/util/geo/Point.h
@@ -23,6 +23,10 @@ class Point {
     return Point<T>(_x + p.getX(), _y + p.getY());
   }
 
+  Point<T> operator-(const Point<T>& p) const {
+    return Point<T>(_x - p.getX(), _y - p.getY());
+  }
+
   bool operator==(const Point<T>& p) const {
     return p.getX() == _x && p.getY() == _y;
   }
diff --git a/src/util/geo/Polygon.h b/src/util/geo/Polygon.h
index 1cef21f..3b1adac 100644
--- a/src/util/geo/Polygon.h
+++ b/src/util/geo/Polygon.h
@@ -6,8 +6,9 @@
 #define UTIL_GEO_POLYGON_H_
 
 #include <vector>
-#include "./Point.h"
+#include "./Box.h"
 #include "./Line.h"
+#include "./Point.h"
 
 namespace util {
 namespace geo {
@@ -18,15 +19,19 @@ class Polygon {
   Polygon() {}
 
   Polygon(const Line<T>& l) : _outer(l) {}
+  Polygon(const Box<T>& b)
+      : _outer({b.getLowerLeft(),
+                Point<T>(b.getUpperRight().getX(), b.getLowerLeft().getY()),
+                b.getUpperRight(),
+                Point<T>(b.getLowerLeft().getX(), b.getUpperRight().getY())}) {}
 
   const std::vector<Point<T>>& getOuter() const { return _outer; }
-  std::vector<Point<T>>& getOuter() {return _outer; }
+  std::vector<Point<T>>& getOuter() { return _outer; }
 
  private:
   std::vector<Point<T>> _outer;
 };
 
-
 }  // namespace geo
 }  // namespace util
 
diff --git a/src/util/tests/TestMain.cpp b/src/util/tests/TestMain.cpp
index 053f20f..cd43ed3 100644
--- a/src/util/tests/TestMain.cpp
+++ b/src/util/tests/TestMain.cpp
@@ -959,12 +959,26 @@ CASE("geometry") {
   EXPECT(geo::contains(Line<double>{Point<double>(4, 2), Point<double>(5, 1)}, poly));
 
   Box<double> polybox(Point<double>(1, 1), Point<double>(6, 4));
+  EXPECT(geo::centroid(polybox).getX() == approx(3.5));
+  EXPECT(geo::centroid(polybox).getY() == approx(2.5));
   EXPECT(geo::contains(poly, polybox));
   EXPECT(!geo::contains(polybox, poly));
   Box<double> polybox2(Point<double>(4, 1), Point<double>(5, 2));
   EXPECT(geo::contains(polybox2, poly));
   EXPECT(geo::contains(poly, getBoundingBox(poly)));
 
+  Point<double> rotP(2, 2);
+  EXPECT(geo::dist(geo::rotate(rotP, 180, Point<double>(1, 1)), Point<double>(0, 0)) == approx(0));
+  EXPECT(geo::dist(geo::rotate(rotP, 360, Point<double>(1, 1)), rotP) == approx(0));
+
+  Line<double> rotLine({{1, 1}, {3, 3}});
+  EXPECT(geo::rotate(rotLine, 90, Point<double>(2, 2))[0].getX() == approx(1));
+  EXPECT(geo::rotate(rotLine, 90, Point<double>(2, 2))[0].getY() == approx(3));
+  EXPECT(geo::rotate(rotLine, 90, Point<double>(2, 2))[1].getX() == approx(3));
+  EXPECT(geo::rotate(rotLine, 90, Point<double>(2, 2))[1].getY() == approx(1));
+
+  EXPECT(geo::dist(geo::centroid(rotP), rotP) == approx(0));
+  EXPECT(geo::dist(geo::centroid(rotLine), rotP) == approx(0));
 }
 
 };