diff --git a/LibCarla/source/carla/geom/Math.h b/LibCarla/source/carla/geom/Math.h
index 1d93c18a2..06f0cdc61 100644
--- a/LibCarla/source/carla/geom/Math.h
+++ b/LibCarla/source/carla/geom/Math.h
@@ -28,7 +28,7 @@ namespace geom {
       return std::min(std::max(a, 0.0), 1.0);
     }
 
-    template<typename T>
+    template <typename T>
     static T sqr(const T &a) {
       return a * a;
     }
@@ -69,6 +69,26 @@ namespace geom {
       const Location projection = v + t * (w - v);
       return Distance2D(p, projection);
     }
+
+    static Location RotatePointOnOrigin2D(Location p, double angle) {
+      double s = std::sin(angle);
+      double c = std::cos(angle);
+      return Location(p.x * c - p.y * s, p.x * s + p.y * c, 0.0);
+    }
+
+    static bool PointInRectangle(
+        const Location &pos,
+        const Location &extent,
+        double angle, // [radians]
+        const Location &p) {
+      // Move p relative to pos's position and angle
+      Location transf_p = RotatePointOnOrigin2D(p - pos, -angle);
+      if (transf_p.x <=  extent.x && transf_p.y <=  extent.y &&
+          transf_p.x >= -extent.x && transf_p.y >= -extent.y) {
+        return true;
+      }
+      return false;
+    }
   };
 
 } // namespace geom
diff --git a/LibCarla/source/test/test_geom.cpp b/LibCarla/source/test/test_geom.cpp
index 5af0d3bb2..86e70400e 100644
--- a/LibCarla/source/test/test_geom.cpp
+++ b/LibCarla/source/test/test_geom.cpp
@@ -71,3 +71,42 @@ TEST(geom, nearest_point_segment) {
     ASSERT_EQ(id, results[i]) << "Fails point number " << i;
   }
 }
+
+TEST(geom, point_in_rectangle) {
+  ASSERT_TRUE(Math::PointInRectangle(
+      Location(0, 0, 0), Location(1, 1, 0), 0, Location(0, 0, 0)));
+  ASSERT_TRUE(Math::PointInRectangle(
+      Location(0, 0, 0), Location(1, 1, 0), 0, Location(1, 1, 0)));
+  ASSERT_TRUE(Math::PointInRectangle(
+      Location(0, 0, 0), Location(1, 1, 0), 0, Location(-1, 1, 0)));
+  ASSERT_TRUE(Math::PointInRectangle(
+      Location(0, 0, 0), Location(1, 1, 0), 0, Location(1, -1, 0)));
+  ASSERT_TRUE(Math::PointInRectangle(
+      Location(0, 0, 0), Location(1, 1, 0), 0, Location(-1, -1, 0)));
+  ASSERT_FALSE(Math::PointInRectangle(
+      Location(0, 0, 0), Location(1, 1, 0), 0, Location(-1.01, -1.01, 0)));
+  ASSERT_FALSE(Math::PointInRectangle(
+      Location(0, 0, 0), Location(1, 1, 0), 0, Location(1.01, 1.01, 0)));
+  ASSERT_FALSE(Math::PointInRectangle(
+      Location(1.5, 1.5, 0), Location(1, 1, 0), 0, Location(0, 0, 0)));
+  ASSERT_TRUE(Math::PointInRectangle(
+      Location(1.5, 1.5, 0), Location(1, 1, 0), 0, Location(1, 1, 0)));
+  ASSERT_TRUE(Math::PointInRectangle(
+      Location(1.5, 1.5, 0), Location(1, 1, 0), 0, Location(2, 1, 0)));
+  ASSERT_TRUE(Math::PointInRectangle(
+      Location(1.5, 1.5, 0), Location(1, 1, 0), 0, Location(2, 1, 0)));
+  ASSERT_TRUE(Math::PointInRectangle(
+      Location(1.5, 1.5, 0), Location(1, 1, 0), 0, Location(2, 2, 0)));
+  ASSERT_TRUE(Math::PointInRectangle(
+      Location(1.5, 1.5, 0), Location(1, 1, 0), 0, Location(1, 1, 0)));
+  ASSERT_FALSE(Math::PointInRectangle(
+      Location(0, 0, 0), Location(1, 1, 0), Math::pi_half() * 0.5, Location(1, 1, 0)));
+  ASSERT_TRUE(Math::PointInRectangle(
+      Location(0, 0, 0), Location(1, 1, 0), Math::pi_half() * 0.5, Location(1, 0, 0)));
+  ASSERT_TRUE(Math::PointInRectangle(
+      Location(0, 2, 0), Location(0.5, 2, 0), Math::pi_half(), Location(2, 2, 0)));
+  ASSERT_FALSE(Math::PointInRectangle(
+      Location(0, 2, 0), Location(0.5, 2, 0), Math::pi_half(), Location(2.1, 2, 0)));
+  ASSERT_FALSE(Math::PointInRectangle(
+      Location(0, 2, 0), Location(0.5, 2, 0), Math::pi_half(), Location(2, 2.6, 0)));
+}