From 601a1c8dcf73f1a419c1ed5c39fbd14c581abf50 Mon Sep 17 00:00:00 2001 From: Chris Loer Date: Thu, 9 Nov 2017 13:24:43 -0800 Subject: [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. --- src/mbgl/geometry/feature_index.cpp | 2 +- src/mbgl/util/grid_index.cpp | 293 +++++++++++++++++++++++++++++++----- src/mbgl/util/grid_index.hpp | 96 ++++++++++-- 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 #include +#include namespace mbgl { template -GridIndex::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::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 void GridIndex::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 +void GridIndex::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 std::vector GridIndex::query(const BBox& queryBBox) const { std::vector result; - std::unordered_set seenUids; + query(queryBBox, [&](const T& t, const BBox&) -> bool { + result.push_back(t); + return false; + }); + return result; +} + +template +std::vector::BBox>> GridIndex::queryWithBoxes(const BBox& queryBBox) const { + std::vector> result; + query(queryBBox, [&](const T& t, const BBox& bbox) -> bool { + result.push_back(std::make_pair(t, bbox)); + return false; + }); + return result; +} + +template +bool GridIndex::hitTest(const BBox& queryBBox) const { + bool hit = false; + query(queryBBox, [&](const T&, const BBox&) -> bool { + hit = true; + return true; + }); + return hit; +} + +template +bool GridIndex::hitTest(const BCircle& queryBCircle) const { + bool hit = false; + query(queryBCircle, [&](const T&, const BBox&) -> bool { + hit = true; + return true; + }); + return hit; +} + +template +bool GridIndex::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 +bool GridIndex::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 +typename GridIndex::BBox GridIndex::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 +void GridIndex::query(const BBox& queryBBox, std::function resultFn) const { + std::unordered_set seenBoxes; + std::unordered_set 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 +void GridIndex::query(const BCircle& queryBCircle, std::function resultFn) const { + std::unordered_set seenBoxes; + std::unordered_set 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 +int16_t GridIndex::convertToXCellCoord(const float x) const { + return util::max(0.0, util::min(xCellCount - 1.0, std::floor(x * xScale))); +} template -int32_t GridIndex::convertToCellCoord(int32_t x) const { - return util::max(0.0, util::min(d - 1.0, std::floor(x * scale) + padding)); +int16_t GridIndex::convertToYCellCoord(const float y) const { + return util::max(0.0, util::min(yCellCount - 1.0, std::floor(y * yScale))); } +template +bool GridIndex::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 +bool GridIndex::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 +bool GridIndex::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 +bool GridIndex::empty() const { + return boxElements.empty() && circleElements.empty(); +} + + template class GridIndex; + } // 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 #include #include +#include namespace mbgl { +namespace geometry { + +template +struct circle +{ + using point_type = mapbox::geometry::point; + + constexpr circle(point_type const& center_, T const& radius_) + : center(center_), radius(radius_) + {} + + point_type center; + T radius; +}; + +template +constexpr bool operator==(circle const& lhs, circle const& rhs) +{ + return lhs.center == rhs.center && lhs.radius == rhs.radius; +} + +template +constexpr bool operator!=(circle const& lhs, circle 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 GridIndex { public: - GridIndex(int32_t extent_, int32_t n_, int32_t padding_); - using BBox = mapbox::geometry::box; + GridIndex(const float width_, const float height_, const int16_t cellSize_); + + using BBox = mapbox::geometry::box; + using BCircle = geometry::circle; void insert(T&& t, const BBox&); + void insert(T&& t, const BCircle&); + std::vector query(const BBox&) const; + std::vector> 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> elements; - std::vector> 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) const; + void query(const BCircle&, std::function) 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> boxElements; + std::vector> circleElements; + + std::vector> boxCells; + std::vector> circleCells; }; -- cgit v1.2.1