[core] Add circle geometries to GridIndex.

 - Adds early exiting "hitTest" query for fast collision detection
 - GridIndex now determines cell count separately for x and y axes based on grid dimensions.

3 files changed, 339 insertions, 52 deletions

diff --git a/src/mbgl/geometry/feature_index.cpp b/src/mbgl/geometry/feature_index.cpp
index 1adb933e44..f7dbbfb8b3 100644
--- a/src/mbgl/geometry/feature_index.cpp
+++ b/src/mbgl/geometry/feature_index.cpp
@@ -18,7 +18,7 @@
 namespace mbgl {
 
 FeatureIndex::FeatureIndex()
-    : grid(util::EXTENT, 16, 0) {
+    : grid(util::EXTENT, util::EXTENT, util::EXTENT >> 5) {
 }
 
 void FeatureIndex::insert(const GeometryCollection& geometries,
diff --git a/src/mbgl/util/grid_index.cpp b/src/mbgl/util/grid_index.cpp
index b3afd3fdc8..88ec1f2b86 100644
--- a/src/mbgl/util/grid_index.cpp
+++ b/src/mbgl/util/grid_index.cpp
@@ -3,83 +3,302 @@
 #include <mbgl/math/minmax.hpp>
 
 #include <unordered_set>
+#include <cmath>
 
 namespace mbgl {
 
 
 template <class T>
-GridIndex<T>::GridIndex(int32_t extent_, int32_t n_, int32_t padding_) :
-    extent(extent_),
-    n(n_),
-    padding(padding_),
-    d(n + 2 * padding),
-    scale(double(n) / double(extent)),
-    min(-double(padding) / n * extent),
-    max(extent + double(padding) / n * extent)
+GridIndex<T>::GridIndex(const float width_, const float height_, const int16_t cellSize_) :
+    width(width_),
+    height(height_),
+    xCellCount(std::ceil(width_ / cellSize_)),
+    yCellCount(std::ceil(height_ / cellSize_)),
+    xScale(xCellCount / width_),
+    yScale(yCellCount / height_)
     {
-        cells.resize(d * d);
+        boxCells.resize(xCellCount * yCellCount);
+        circleCells.resize(xCellCount * yCellCount);
     }
 
 template <class T>
 void GridIndex<T>::insert(T&& t, const BBox& bbox) {
-    size_t uid = elements.size();
+    size_t uid = boxElements.size();
 
-    auto cx1 = convertToCellCoord(bbox.min.x);
-    auto cy1 = convertToCellCoord(bbox.min.y);
-    auto cx2 = convertToCellCoord(bbox.max.x);
-    auto cy2 = convertToCellCoord(bbox.max.y);
+    auto cx1 = convertToXCellCoord(bbox.min.x);
+    auto cy1 = convertToYCellCoord(bbox.min.y);
+    auto cx2 = convertToXCellCoord(bbox.max.x);
+    auto cy2 = convertToYCellCoord(bbox.max.y);
 
-    int32_t x, y, cellIndex;
+    int16_t x, y, cellIndex;
     for (x = cx1; x <= cx2; ++x) {
         for (y = cy1; y <= cy2; ++y) {
-            cellIndex = d * y + x;
-            cells[cellIndex].push_back(uid);
+            cellIndex = xCellCount * y + x;
+            boxCells[cellIndex].push_back(uid);
         }
     }
 
-    elements.emplace_back(t, bbox);
+    boxElements.emplace_back(t, bbox);
+}
+
+template <class T>
+void GridIndex<T>::insert(T&& t, const BCircle& bcircle) {
+    size_t uid = circleElements.size();
+
+    auto cx1 = convertToXCellCoord(bcircle.center.x - bcircle.radius);
+    auto cy1 = convertToYCellCoord(bcircle.center.y - bcircle.radius);
+    auto cx2 = convertToXCellCoord(bcircle.center.x + bcircle.radius);
+    auto cy2 = convertToYCellCoord(bcircle.center.y + bcircle.radius);
+
+    int16_t x, y, cellIndex;
+    for (x = cx1; x <= cx2; ++x) {
+        for (y = cy1; y <= cy2; ++y) {
+            cellIndex = xCellCount * y + x;
+            circleCells[cellIndex].push_back(uid);
+        }
+    }
+
+    circleElements.emplace_back(t, bcircle);
 }
 
 template <class T>
 std::vector<T> GridIndex<T>::query(const BBox& queryBBox) const {
     std::vector<T> result;
-    std::unordered_set<size_t> seenUids;
+    query(queryBBox, [&](const T& t, const BBox&) -> bool {
+        result.push_back(t);
+        return false;
+    });
+    return result;
+}
+
+template <class T>
+std::vector<std::pair<T, typename GridIndex<T>::BBox>> GridIndex<T>::queryWithBoxes(const BBox& queryBBox) const {
+    std::vector<std::pair<T, BBox>> result;
+    query(queryBBox, [&](const T& t, const BBox& bbox) -> bool {
+        result.push_back(std::make_pair(t, bbox));
+        return false;
+    });
+    return result;
+}
+
+template <class T>
+bool GridIndex<T>::hitTest(const BBox& queryBBox) const {
+    bool hit = false;
+    query(queryBBox, [&](const T&, const BBox&) -> bool {
+        hit = true;
+        return true;
+    });
+    return hit;
+}
+
+template <class T>
+bool GridIndex<T>::hitTest(const BCircle& queryBCircle) const {
+    bool hit = false;
+    query(queryBCircle, [&](const T&, const BBox&) -> bool {
+        hit = true;
+        return true;
+    });
+    return hit;
+}
+
+template <class T>
+bool GridIndex<T>::noIntersection(const BBox& queryBBox) const {
+    return queryBBox.max.x < 0 || queryBBox.min.x > width || queryBBox.max.y < 0 || queryBBox.min.y > height;
+}
 
-    auto cx1 = convertToCellCoord(queryBBox.min.x);
-    auto cy1 = convertToCellCoord(queryBBox.min.y);
-    auto cx2 = convertToCellCoord(queryBBox.max.x);
-    auto cy2 = convertToCellCoord(queryBBox.max.y);
+template <class T>
+bool GridIndex<T>::completeIntersection(const BBox& queryBBox) const {
+    return queryBBox.min.x <= 0 && queryBBox.min.y <= 0 && width <= queryBBox.max.x && height <= queryBBox.max.y;
+}
 
-    int32_t x, y, cellIndex;
+template <class T>
+typename GridIndex<T>::BBox GridIndex<T>::convertToBox(const BCircle& circle) const {
+    return BBox{{circle.center.x - circle.radius, circle.center.y - circle.radius},
+                {circle.center.x + circle.radius, circle.center.y + circle.radius}};
+}
+
+template <class T>
+void GridIndex<T>::query(const BBox& queryBBox, std::function<bool (const T&, const BBox&)> resultFn) const {
+    std::unordered_set<size_t> seenBoxes;
+    std::unordered_set<size_t> seenCircles;
+    
+    if (noIntersection(queryBBox)) {
+        return;
+    } else if (completeIntersection(queryBBox)) {
+        for (auto& element : boxElements) {
+            if (resultFn(element.first, element.second)) {
+                return;
+            };
+        }
+        for (auto& element : circleElements) {
+            if (resultFn(element.first, convertToBox(element.second))) {
+                return;
+            };
+        }
+        return;
+    }
+
+    auto cx1 = convertToXCellCoord(queryBBox.min.x);
+    auto cy1 = convertToYCellCoord(queryBBox.min.y);
+    auto cx2 = convertToXCellCoord(queryBBox.max.x);
+    auto cy2 = convertToYCellCoord(queryBBox.max.y);
+
+    int16_t x, y, cellIndex;
     for (x = cx1; x <= cx2; ++x) {
         for (y = cy1; y <= cy2; ++y) {
-            cellIndex = d * y + x;
-            for (auto uid : cells[cellIndex]) {
-                if (seenUids.count(uid) == 0) {
-                    seenUids.insert(uid);
+            cellIndex = xCellCount * y + x;
+            // Look up other boxes
+            for (auto uid : boxCells[cellIndex]) {
+                if (seenBoxes.count(uid) == 0) {
+                    seenBoxes.insert(uid);
 
-                    auto& pair = elements.at(uid);
+                    auto& pair = boxElements.at(uid);
                     auto& bbox = pair.second;
-                    if (queryBBox.min.x <= bbox.max.x &&
-                        queryBBox.min.y <= bbox.max.y &&
-                        queryBBox.max.x >= bbox.min.x &&
-                        queryBBox.max.y >= bbox.min.y) {
+                    if (boxesCollide(queryBBox, bbox)) {
+                        if (resultFn(pair.first, bbox)) {
+                            return;
+                        }
+                    }
+                }
+            }
+            
+            // Look up circles
+            for (auto uid : circleCells[cellIndex]) {
+                if (seenCircles.count(uid) == 0) {
+                    seenCircles.insert(uid);
 
-                        result.push_back(pair.first);
+                    auto& pair = circleElements.at(uid);
+                    auto& bcircle = pair.second;
+                    if (circleAndBoxCollide(bcircle, queryBBox)) {
+                        if (resultFn(pair.first, convertToBox(bcircle))) {
+                            return;
+                        }
                     }
                 }
             }
         }
     }
+}
 
-    return result;
+template <class T>
+void GridIndex<T>::query(const BCircle& queryBCircle, std::function<bool (const T&, const BBox&)> resultFn) const {
+    std::unordered_set<size_t> seenBoxes;
+    std::unordered_set<size_t> seenCircles;
+
+    BBox queryBBox = convertToBox(queryBCircle);
+    if (noIntersection(queryBBox)) {
+        return;
+    } else if (completeIntersection(queryBBox)) {
+        // TODO: std::for_each?
+        for (auto& element : boxElements) {
+            if (resultFn(element.first, element.second)) {
+                return;
+            };
+        }
+        for (auto& element : circleElements) {
+            if (resultFn(element.first, convertToBox(element.second))) {
+                return;
+            };
+        }
+    }
+
+    auto cx1 = convertToXCellCoord(queryBCircle.center.x - queryBCircle.radius);
+    auto cy1 = convertToYCellCoord(queryBCircle.center.y - queryBCircle.radius);
+    auto cx2 = convertToXCellCoord(queryBCircle.center.x + queryBCircle.radius);
+    auto cy2 = convertToYCellCoord(queryBCircle.center.y + queryBCircle.radius);
+
+    int16_t x, y, cellIndex;
+    for (x = cx1; x <= cx2; ++x) {
+        for (y = cy1; y <= cy2; ++y) {
+            cellIndex = xCellCount * y + x;
+            // Look up boxes
+            for (auto uid : boxCells[cellIndex]) {
+                if (seenBoxes.count(uid) == 0) {
+                    seenBoxes.insert(uid);
+
+                    auto& pair = boxElements.at(uid);
+                    auto& bbox = pair.second;
+                    if (circleAndBoxCollide(queryBCircle, bbox)) {
+                        if (resultFn(pair.first, bbox)) {
+                            return;
+                        }
+                    }
+                }
+            }
+            
+            // Look up other circles
+            for (auto uid : circleCells[cellIndex]) {
+                if (seenCircles.count(uid) == 0) {
+                    seenCircles.insert(uid);
+
+                    auto& pair = circleElements.at(uid);
+                    auto& bcircle = pair.second;
+                    if (circlesCollide(queryBCircle, bcircle)) {
+                        if (resultFn(pair.first, convertToBox(bcircle))) {
+                            return;
+                        }
+                    }
+                }
+            }
+        }
+    }
 }
 
+template <class T>
+int16_t GridIndex<T>::convertToXCellCoord(const float x) const {
+    return util::max(0.0, util::min(xCellCount - 1.0, std::floor(x * xScale)));
+}
 
 template <class T>
-int32_t GridIndex<T>::convertToCellCoord(int32_t x) const {
-    return util::max(0.0, util::min(d - 1.0, std::floor(x * scale) + padding));
+int16_t GridIndex<T>::convertToYCellCoord(const float y) const {
+    return util::max(0.0, util::min(yCellCount - 1.0, std::floor(y * yScale)));
 }
 
+template <class T>
+bool GridIndex<T>::boxesCollide(const BBox& first, const BBox& second) const {
+	return first.min.x <= second.max.x &&
+           first.min.y <= second.max.y &&
+           first.max.x >= second.min.x &&
+           first.max.y >= second.min.y;
+}
+
+template <class T>
+bool GridIndex<T>::circlesCollide(const BCircle& first, const BCircle& second) const {
+    auto dx = second.center.x - first.center.x;
+    auto dy = second.center.y - first.center.y;
+    auto bothRadii = first.radius + second.radius;
+    return (bothRadii * bothRadii) > (dx * dx + dy * dy);
+}
+
+template <class T>
+bool GridIndex<T>::circleAndBoxCollide(const BCircle& circle, const BBox& box) const {
+    auto halfRectWidth = (box.max.x - box.min.x) / 2;
+    auto distX = std::abs(circle.center.x - (box.min.x + halfRectWidth));
+    if (distX > (halfRectWidth + circle.radius)) {
+        return false;
+    }
+
+    auto halfRectHeight = (box.max.y - box.min.y) / 2;
+    auto distY = std::abs(circle.center.y - (box.min.y + halfRectHeight));
+    if (distY > (halfRectHeight + circle.radius)) {
+        return false;
+    }
+
+    if (distX <= halfRectWidth || distY <= halfRectHeight) {
+        return true;
+    }
+
+    auto dx = distX - halfRectWidth;
+    auto dy = distY - halfRectHeight;
+    return (dx * dx + dy * dy) <= (circle.radius * circle.radius);
+}
+
+template <class T>
+bool GridIndex<T>::empty() const {
+    return boxElements.empty() && circleElements.empty();
+}
+
+
 template class GridIndex<IndexedSubfeature>;
+
 } // namespace mbgl
diff --git a/src/mbgl/util/grid_index.hpp b/src/mbgl/util/grid_index.hpp
index 8ef8fb35b7..6ef2966bee 100644
--- a/src/mbgl/util/grid_index.hpp
+++ b/src/mbgl/util/grid_index.hpp
@@ -6,32 +6,100 @@
 #include <cstdint>
 #include <cstddef>
 #include <vector>
+#include <functional>
 
 namespace mbgl {
 
+namespace geometry {
+
+template <typename T>
+struct circle
+{
+    using point_type = mapbox::geometry::point<T>;
+
+    constexpr circle(point_type const& center_, T const& radius_)
+        : center(center_), radius(radius_)
+    {}
+
+    point_type center;
+    T radius;
+};
+
+template <typename T>
+constexpr bool operator==(circle<T> const& lhs, circle<T> const& rhs)
+{
+    return lhs.center == rhs.center && lhs.radius == rhs.radius;
+}
+
+template <typename T>
+constexpr bool operator!=(circle<T> const& lhs, circle<T> const& rhs)
+{
+    return lhs.center != rhs.center || lhs.radius != rhs.radius;
+}
+
+} // namespace geometry
+
+
+/*
+ GridIndex is a data structure for testing the intersection of
+ circles and rectangles in a 2d plane.
+ It is optimized for rapid insertion and querying.
+ GridIndex splits the plane into a set of "cells" and keeps track
+ of which geometries intersect with each cell. At query time,
+ full geometry comparisons are only done for items that share
+ at least one cell. As long as the geometries are relatively
+ uniformly distributed across the plane, this greatly reduces
+ the number of comparisons necessary.
+*/
+
 template <class T>
 class GridIndex {
 public:
-    GridIndex(int32_t extent_, int32_t n_, int32_t padding_);
 
-    using BBox = mapbox::geometry::box<int16_t>;
+    GridIndex(const float width_, const float height_, const int16_t cellSize_);
+
+    using BBox = mapbox::geometry::box<float>;
+    using BCircle = geometry::circle<float>;
 
     void insert(T&& t, const BBox&);
+    void insert(T&& t, const BCircle&);
+    
     std::vector<T> query(const BBox&) const;
+    std::vector<std::pair<T,BBox>> queryWithBoxes(const BBox&) const;
+    
+    bool hitTest(const BBox&) const;
+    bool hitTest(const BCircle&) const;
+    
+    bool empty() const;
 
 private:
-    int32_t convertToCellCoord(int32_t x) const;
-
-    const int32_t extent;
-    const int32_t n;
-    const int32_t padding;
-    const int32_t d;
-    const double scale;
-    const int32_t min;
-    const int32_t max;
-
-    std::vector<std::pair<T, BBox>> elements;
-    std::vector<std::vector<size_t>> cells;
+    bool noIntersection(const BBox& queryBBox) const;
+    bool completeIntersection(const BBox& queryBBox) const;
+    BBox convertToBox(const BCircle& circle) const;
+
+    void query(const BBox&, std::function<bool (const T&, const BBox&)>) const;
+    void query(const BCircle&, std::function<bool (const T&, const BBox&)>) const;
+
+    int16_t convertToXCellCoord(const float x) const;
+    int16_t convertToYCellCoord(const float y) const;
+    
+    bool boxesCollide(const BBox&, const BBox&) const;
+    bool circlesCollide(const BCircle&, const BCircle&) const;
+    bool circleAndBoxCollide(const BCircle&, const BBox&) const;
+
+    const float width;
+    const float height;
+    
+    const int16_t xCellCount;
+    const int16_t yCellCount;
+    const double xScale;
+    const double yScale;
+
+    std::vector<std::pair<T, BBox>> boxElements;
+    std::vector<std::pair<T, BCircle>> circleElements;
+    
+    std::vector<std::vector<size_t>> boxCells;
+    std::vector<std::vector<size_t>> circleCells;
 
 };