1 #include <mbgl/util/grid_index.hpp>
2 #include <mbgl/geometry/feature_index.hpp>
3 #include <mbgl/math/minmax.hpp>
4
5 #include <unordered_set>
6 #include <cmath>
7
8 namespace mbgl {
9
10
11 template <class T>
GridIndex(const float width_,const float height_,const int16_t cellSize_)12 GridIndex<T>::GridIndex(const float width_, const float height_, const int16_t cellSize_) :
13 width(width_),
14 height(height_),
15 xCellCount(std::ceil(width_ / cellSize_)),
16 yCellCount(std::ceil(height_ / cellSize_)),
17 xScale(xCellCount / width_),
18 yScale(yCellCount / height_)
19 {
20 boxCells.resize(xCellCount * yCellCount);
21 circleCells.resize(xCellCount * yCellCount);
22 }
23
24 template <class T>
insert(T && t,const BBox & bbox)25 void GridIndex<T>::insert(T&& t, const BBox& bbox) {
26 size_t uid = boxElements.size();
27
28 auto cx1 = convertToXCellCoord(bbox.min.x);
29 auto cy1 = convertToYCellCoord(bbox.min.y);
30 auto cx2 = convertToXCellCoord(bbox.max.x);
31 auto cy2 = convertToYCellCoord(bbox.max.y);
32
33 int16_t x, y, cellIndex;
34 for (x = cx1; x <= cx2; ++x) {
35 for (y = cy1; y <= cy2; ++y) {
36 cellIndex = xCellCount * y + x;
37 boxCells[cellIndex].push_back(uid);
38 }
39 }
40
41 boxElements.emplace_back(t, bbox);
42 }
43
44 template <class T>
insert(T && t,const BCircle & bcircle)45 void GridIndex<T>::insert(T&& t, const BCircle& bcircle) {
46 size_t uid = circleElements.size();
47
48 auto cx1 = convertToXCellCoord(bcircle.center.x - bcircle.radius);
49 auto cy1 = convertToYCellCoord(bcircle.center.y - bcircle.radius);
50 auto cx2 = convertToXCellCoord(bcircle.center.x + bcircle.radius);
51 auto cy2 = convertToYCellCoord(bcircle.center.y + bcircle.radius);
52
53 int16_t x, y, cellIndex;
54 for (x = cx1; x <= cx2; ++x) {
55 for (y = cy1; y <= cy2; ++y) {
56 cellIndex = xCellCount * y + x;
57 circleCells[cellIndex].push_back(uid);
58 }
59 }
60
61 circleElements.emplace_back(t, bcircle);
62 }
63
64 template <class T>
query(const BBox & queryBBox) const65 std::vector<T> GridIndex<T>::query(const BBox& queryBBox) const {
66 std::vector<T> result;
67 query(queryBBox, [&](const T& t, const BBox&) -> bool {
68 result.push_back(t);
69 return false;
70 });
71 return result;
72 }
73
74 template <class T>
queryWithBoxes(const BBox & queryBBox) const75 std::vector<std::pair<T, typename GridIndex<T>::BBox>> GridIndex<T>::queryWithBoxes(const BBox& queryBBox) const {
76 std::vector<std::pair<T, BBox>> result;
77 query(queryBBox, [&](const T& t, const BBox& bbox) -> bool {
78 result.push_back(std::make_pair(t, bbox));
79 return false;
80 });
81 return result;
82 }
83
84 template <class T>
hitTest(const BBox & queryBBox) const85 bool GridIndex<T>::hitTest(const BBox& queryBBox) const {
86 bool hit = false;
87 query(queryBBox, [&](const T&, const BBox&) -> bool {
88 hit = true;
89 return true;
90 });
91 return hit;
92 }
93
94 template <class T>
hitTest(const BCircle & queryBCircle) const95 bool GridIndex<T>::hitTest(const BCircle& queryBCircle) const {
96 bool hit = false;
97 query(queryBCircle, [&](const T&, const BBox&) -> bool {
98 hit = true;
99 return true;
100 });
101 return hit;
102 }
103
104 template <class T>
noIntersection(const BBox & queryBBox) const105 bool GridIndex<T>::noIntersection(const BBox& queryBBox) const {
106 return queryBBox.max.x < 0 || queryBBox.min.x >= width || queryBBox.max.y < 0 || queryBBox.min.y >= height;
107 }
108
109 template <class T>
completeIntersection(const BBox & queryBBox) const110 bool GridIndex<T>::completeIntersection(const BBox& queryBBox) const {
111 return queryBBox.min.x <= 0 && queryBBox.min.y <= 0 && width <= queryBBox.max.x && height <= queryBBox.max.y;
112 }
113
114 template <class T>
convertToBox(const BCircle & circle) const115 typename GridIndex<T>::BBox GridIndex<T>::convertToBox(const BCircle& circle) const {
116 return BBox{{circle.center.x - circle.radius, circle.center.y - circle.radius},
117 {circle.center.x + circle.radius, circle.center.y + circle.radius}};
118 }
119
120 template <class T>
query(const BBox & queryBBox,std::function<bool (const T &,const BBox &)> resultFn) const121 void GridIndex<T>::query(const BBox& queryBBox, std::function<bool (const T&, const BBox&)> resultFn) const {
122 std::unordered_set<size_t> seenBoxes;
123 std::unordered_set<size_t> seenCircles;
124
125 if (noIntersection(queryBBox)) {
126 return;
127 } else if (completeIntersection(queryBBox)) {
128 for (auto& element : boxElements) {
129 if (resultFn(element.first, element.second)) {
130 return;
131 }
132 }
133 for (auto& element : circleElements) {
134 if (resultFn(element.first, convertToBox(element.second))) {
135 return;
136 }
137 }
138 return;
139 }
140
141 auto cx1 = convertToXCellCoord(queryBBox.min.x);
142 auto cy1 = convertToYCellCoord(queryBBox.min.y);
143 auto cx2 = convertToXCellCoord(queryBBox.max.x);
144 auto cy2 = convertToYCellCoord(queryBBox.max.y);
145
146 int16_t x, y, cellIndex;
147 for (x = cx1; x <= cx2; ++x) {
148 for (y = cy1; y <= cy2; ++y) {
149 cellIndex = xCellCount * y + x;
150 // Look up other boxes
151 for (auto uid : boxCells[cellIndex]) {
152 if (seenBoxes.count(uid) == 0) {
153 seenBoxes.insert(uid);
154
155 auto& pair = boxElements.at(uid);
156 auto& bbox = pair.second;
157 if (boxesCollide(queryBBox, bbox)) {
158 if (resultFn(pair.first, bbox)) {
159 return;
160 }
161 }
162 }
163 }
164
165 // Look up circles
166 for (auto uid : circleCells[cellIndex]) {
167 if (seenCircles.count(uid) == 0) {
168 seenCircles.insert(uid);
169
170 auto& pair = circleElements.at(uid);
171 auto& bcircle = pair.second;
172 if (circleAndBoxCollide(bcircle, queryBBox)) {
173 if (resultFn(pair.first, convertToBox(bcircle))) {
174 return;
175 }
176 }
177 }
178 }
179 }
180 }
181 }
182
183 template <class T>
query(const BCircle & queryBCircle,std::function<bool (const T &,const BBox &)> resultFn) const184 void GridIndex<T>::query(const BCircle& queryBCircle, std::function<bool (const T&, const BBox&)> resultFn) const {
185 std::unordered_set<size_t> seenBoxes;
186 std::unordered_set<size_t> seenCircles;
187
188 BBox queryBBox = convertToBox(queryBCircle);
189 if (noIntersection(queryBBox)) {
190 return;
191 } else if (completeIntersection(queryBBox)) {
192 for (auto& element : boxElements) {
193 if (resultFn(element.first, element.second)) {
194 return;
195 }
196 }
197 for (auto& element : circleElements) {
198 if (resultFn(element.first, convertToBox(element.second))) {
199 return;
200 }
201 }
202 }
203
204 auto cx1 = convertToXCellCoord(queryBCircle.center.x - queryBCircle.radius);
205 auto cy1 = convertToYCellCoord(queryBCircle.center.y - queryBCircle.radius);
206 auto cx2 = convertToXCellCoord(queryBCircle.center.x + queryBCircle.radius);
207 auto cy2 = convertToYCellCoord(queryBCircle.center.y + queryBCircle.radius);
208
209 int16_t x, y, cellIndex;
210 for (x = cx1; x <= cx2; ++x) {
211 for (y = cy1; y <= cy2; ++y) {
212 cellIndex = xCellCount * y + x;
213 // Look up boxes
214 for (auto uid : boxCells[cellIndex]) {
215 if (seenBoxes.count(uid) == 0) {
216 seenBoxes.insert(uid);
217
218 auto& pair = boxElements.at(uid);
219 auto& bbox = pair.second;
220 if (circleAndBoxCollide(queryBCircle, bbox)) {
221 if (resultFn(pair.first, bbox)) {
222 return;
223 }
224 }
225 }
226 }
227
228 // Look up other circles
229 for (auto uid : circleCells[cellIndex]) {
230 if (seenCircles.count(uid) == 0) {
231 seenCircles.insert(uid);
232
233 auto& pair = circleElements.at(uid);
234 auto& bcircle = pair.second;
235 if (circlesCollide(queryBCircle, bcircle)) {
236 if (resultFn(pair.first, convertToBox(bcircle))) {
237 return;
238 }
239 }
240 }
241 }
242 }
243 }
244 }
245
246 template <class T>
convertToXCellCoord(const float x) const247 int16_t GridIndex<T>::convertToXCellCoord(const float x) const {
248 return util::max(0.0, util::min(xCellCount - 1.0, std::floor(x * xScale)));
249 }
250
251 template <class T>
convertToYCellCoord(const float y) const252 int16_t GridIndex<T>::convertToYCellCoord(const float y) const {
253 return util::max(0.0, util::min(yCellCount - 1.0, std::floor(y * yScale)));
254 }
255
256 template <class T>
boxesCollide(const BBox & first,const BBox & second) const257 bool GridIndex<T>::boxesCollide(const BBox& first, const BBox& second) const {
258 return first.min.x <= second.max.x &&
259 first.min.y <= second.max.y &&
260 first.max.x >= second.min.x &&
261 first.max.y >= second.min.y;
262 }
263
264 template <class T>
circlesCollide(const BCircle & first,const BCircle & second) const265 bool GridIndex<T>::circlesCollide(const BCircle& first, const BCircle& second) const {
266 auto dx = second.center.x - first.center.x;
267 auto dy = second.center.y - first.center.y;
268 auto bothRadii = first.radius + second.radius;
269 return (bothRadii * bothRadii) > (dx * dx + dy * dy);
270 }
271
272 template <class T>
circleAndBoxCollide(const BCircle & circle,const BBox & box) const273 bool GridIndex<T>::circleAndBoxCollide(const BCircle& circle, const BBox& box) const {
274 auto halfRectWidth = (box.max.x - box.min.x) / 2;
275 auto distX = std::abs(circle.center.x - (box.min.x + halfRectWidth));
276 if (distX > (halfRectWidth + circle.radius)) {
277 return false;
278 }
279
280 auto halfRectHeight = (box.max.y - box.min.y) / 2;
281 auto distY = std::abs(circle.center.y - (box.min.y + halfRectHeight));
282 if (distY > (halfRectHeight + circle.radius)) {
283 return false;
284 }
285
286 if (distX <= halfRectWidth || distY <= halfRectHeight) {
287 return true;
288 }
289
290 auto dx = distX - halfRectWidth;
291 auto dy = distY - halfRectHeight;
292 return (dx * dx + dy * dy) <= (circle.radius * circle.radius);
293 }
294
295 template <class T>
empty() const296 bool GridIndex<T>::empty() const {
297 return boxElements.empty() && circleElements.empty();
298 }
299
300
301 template class GridIndex<IndexedSubfeature>;
302
303 } // namespace mbgl
304