[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.
This commit is contained in:
Chris Loer
@@ -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,
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
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>
|
||||
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;
|
||||
}
|
||||
|
||||
int32_t x, y, cellIndex;
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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>
|
||||
int32_t GridIndex<T>::convertToCellCoord(int32_t x) const {
|
||||
return util::max(0.0, util::min(d - 1.0, std::floor(x * scale) + padding));
|
||||
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>
|
||||
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
|
||||
|
||||
@@ -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;
|
||||
bool noIntersection(const BBox& queryBBox) const;
|
||||
bool completeIntersection(const BBox& queryBBox) const;
|
||||
BBox convertToBox(const BCircle& circle) 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;
|
||||
void query(const BBox&, std::function<bool (const T&, const BBox&)>) const;
|
||||
void query(const BCircle&, std::function<bool (const T&, const BBox&)>) const;
|
||||
|
||||
std::vector<std::pair<T, BBox>> elements;
|
||||
std::vector<std::vector<size_t>> cells;
|
||||
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;
|
||||
|
||||
};
|
||||
|
||||
|
||||
Reference in New Issue
Block a user