Line data    Source code

       1             : /******************************************************************************
       2             :  *
       3             :  * Project:  FlatGeobuf
       4             :  * Purpose:  Packed RTree management
       5             :  * Author:   Björn Harrtell <bjorn at wololo dot org>
       6             :  *
       7             :  ******************************************************************************
       8             :  * Copyright (c) 2018-2020, Björn Harrtell <bjorn at wololo dot org>
       9             :  *
      10             :  * SPDX-License-Identifier: MIT
      11             :  ****************************************************************************/
      12             : 
      13             : // NOTE: The upstream of this file is in
      14             : // https://github.com/bjornharrtell/flatgeobuf/tree/master/src/cpp
      15             : 
      16             : #ifdef GDAL_COMPILATION
      17             : #include "cpl_port.h"
      18             : #else
      19             : #define CPL_IS_LSB 1
      20             : #endif
      21             : 
      22             : #include "packedrtree.h"
      23             : 
      24             : #include <algorithm>
      25             : #include <limits>
      26             : #include <map>
      27             : #include <unordered_map>
      28             : #include <iostream>
      29             : 
      30             : namespace FlatGeobuf
      31             : {
      32             : 
      33         483 : const NodeItem &NodeItem::expand(const NodeItem &r)
      34             : {
      35         483 :     if (r.minX < minX)
      36         376 :         minX = r.minX;
      37         483 :     if (r.minY < minY)
      38         359 :         minY = r.minY;
      39         483 :     if (r.maxX > maxX)
      40         380 :         maxX = r.maxX;
      41         483 :     if (r.maxY > maxY)
      42         371 :         maxY = r.maxY;
      43         483 :     return *this;
      44             : }
      45             : 
      46         348 : NodeItem NodeItem::create(uint64_t offset)
      47             : {
      48             :     return {std::numeric_limits<double>::infinity(),
      49             :             std::numeric_limits<double>::infinity(),
      50             :             -1 * std::numeric_limits<double>::infinity(),
      51         348 :             -1 * std::numeric_limits<double>::infinity(), offset};
      52             : }
      53             : 
      54         209 : bool NodeItem::intersects(const NodeItem &r) const
      55             : {
      56         209 :     if (maxX < r.minX)
      57          20 :         return false;
      58         189 :     if (maxY < r.minY)
      59          15 :         return false;
      60         174 :     if (minX > r.maxX)
      61          21 :         return false;
      62         153 :     if (minY > r.maxY)
      63           0 :         return false;
      64         153 :     return true;
      65             : }
      66             : 
      67         116 : std::vector<double> NodeItem::toVector()
      68             : {
      69         116 :     return std::vector<double>{minX, minY, maxX, maxY};
      70             : }
      71             : 
      72             : // Based on public domain code at
      73             : // https://github.com/rawrunprotected/hilbert_curves
      74         240 : uint32_t hilbert(uint32_t x, uint32_t y)
      75             : {
      76         240 :     uint32_t a = x ^ y;
      77         240 :     uint32_t b = 0xFFFF ^ a;
      78         240 :     uint32_t c = 0xFFFF ^ (x | y);
      79         240 :     uint32_t d = x & (y ^ 0xFFFF);
      80             : 
      81         240 :     uint32_t A = a | (b >> 1);
      82         240 :     uint32_t B = (a >> 1) ^ a;
      83         240 :     uint32_t C = ((c >> 1) ^ (b & (d >> 1))) ^ c;
      84         240 :     uint32_t D = ((a & (c >> 1)) ^ (d >> 1)) ^ d;
      85             : 
      86         240 :     a = A;
      87         240 :     b = B;
      88         240 :     c = C;
      89         240 :     d = D;
      90         240 :     A = ((a & (a >> 2)) ^ (b & (b >> 2)));
      91         240 :     B = ((a & (b >> 2)) ^ (b & ((a ^ b) >> 2)));
      92         240 :     C ^= ((a & (c >> 2)) ^ (b & (d >> 2)));
      93         240 :     D ^= ((b & (c >> 2)) ^ ((a ^ b) & (d >> 2)));
      94             : 
      95         240 :     a = A;
      96         240 :     b = B;
      97         240 :     c = C;
      98         240 :     d = D;
      99         240 :     A = ((a & (a >> 4)) ^ (b & (b >> 4)));
     100         240 :     B = ((a & (b >> 4)) ^ (b & ((a ^ b) >> 4)));
     101         240 :     C ^= ((a & (c >> 4)) ^ (b & (d >> 4)));
     102         240 :     D ^= ((b & (c >> 4)) ^ ((a ^ b) & (d >> 4)));
     103             : 
     104         240 :     a = A;
     105         240 :     b = B;
     106         240 :     c = C;
     107         240 :     d = D;
     108         240 :     C ^= ((a & (c >> 8)) ^ (b & (d >> 8)));
     109         240 :     D ^= ((b & (c >> 8)) ^ ((a ^ b) & (d >> 8)));
     110             : 
     111         240 :     a = C ^ (C >> 1);
     112         240 :     b = D ^ (D >> 1);
     113             : 
     114         240 :     uint32_t i0 = x ^ y;
     115         240 :     uint32_t i1 = b | (0xFFFF ^ (i0 | a));
     116             : 
     117         240 :     i0 = (i0 | (i0 << 8)) & 0x00FF00FF;
     118         240 :     i0 = (i0 | (i0 << 4)) & 0x0F0F0F0F;
     119         240 :     i0 = (i0 | (i0 << 2)) & 0x33333333;
     120         240 :     i0 = (i0 | (i0 << 1)) & 0x55555555;
     121             : 
     122         240 :     i1 = (i1 | (i1 << 8)) & 0x00FF00FF;
     123         240 :     i1 = (i1 | (i1 << 4)) & 0x0F0F0F0F;
     124         240 :     i1 = (i1 | (i1 << 2)) & 0x33333333;
     125         240 :     i1 = (i1 | (i1 << 1)) & 0x55555555;
     126             : 
     127         240 :     uint32_t value = ((i1 << 1) | i0);
     128             : 
     129         240 :     return value;
     130             : }
     131             : 
     132         240 : uint32_t hilbert(const NodeItem &r, uint32_t hilbertMax, const double minX,
     133             :                  const double minY, const double width, const double height)
     134             : {
     135         240 :     uint32_t x = 0;
     136         240 :     uint32_t y = 0;
     137             :     uint32_t v;
     138         240 :     if (width != 0.0)
     139         232 :         x = static_cast<uint32_t>(
     140         232 :             floor(hilbertMax * ((r.minX + r.maxX) / 2 - minX) / width));
     141         240 :     if (height != 0.0)
     142         228 :         y = static_cast<uint32_t>(
     143         228 :             floor(hilbertMax * ((r.minY + r.maxY) / 2 - minY) / height));
     144         240 :     v = hilbert(x, y);
     145         240 :     return v;
     146             : }
     147             : 
     148           0 : void hilbertSort(std::vector<std::shared_ptr<Item>> &items)
     149             : {
     150           0 :     NodeItem extent = calcExtent(items);
     151           0 :     const double minX = extent.minX;
     152           0 :     const double minY = extent.minY;
     153           0 :     const double width = extent.width();
     154           0 :     const double height = extent.height();
     155           0 :     std::sort(items.begin(), items.end(),
     156           0 :               [minX, minY, width, height](std::shared_ptr<Item> a,
     157           0 :                                           std::shared_ptr<Item> b)
     158             :               {
     159           0 :                   uint32_t ha = hilbert(a->nodeItem, HILBERT_MAX, minX, minY,
     160             :                                         width, height);
     161           0 :                   uint32_t hb = hilbert(b->nodeItem, HILBERT_MAX, minX, minY,
     162             :                                         width, height);
     163           0 :                   return ha > hb;
     164             :               });
     165           0 : }
     166             : 
     167           0 : void hilbertSort(std::vector<NodeItem> &items)
     168             : {
     169           0 :     NodeItem extent = calcExtent(items);
     170           0 :     const double minX = extent.minX;
     171           0 :     const double minY = extent.minY;
     172           0 :     const double width = extent.width();
     173           0 :     const double height = extent.height();
     174           0 :     std::sort(items.begin(), items.end(),
     175           0 :               [minX, minY, width, height](const NodeItem &a, const NodeItem &b)
     176             :               {
     177             :                   uint32_t ha =
     178           0 :                       hilbert(a, HILBERT_MAX, minX, minY, width, height);
     179             :                   uint32_t hb =
     180           0 :                       hilbert(b, HILBERT_MAX, minX, minY, width, height);
     181           0 :                   return ha > hb;
     182             :               });
     183           0 : }
     184             : 
     185           0 : NodeItem calcExtent(const std::vector<std::shared_ptr<Item>> &items)
     186             : {
     187             :     return std::accumulate(
     188             :         items.begin(), items.end(), NodeItem::create(0),
     189           0 :         [](NodeItem a, const std::shared_ptr<Item> &b) -> NodeItem
     190           0 :         { return a.expand(b->nodeItem); });
     191             : }
     192             : 
     193           0 : NodeItem calcExtent(const std::vector<NodeItem> &nodes)
     194             : {
     195             :     return std::accumulate(nodes.begin(), nodes.end(), NodeItem::create(0),
     196           0 :                            [](NodeItem a, const NodeItem &b) -> NodeItem
     197           0 :                            { return a.expand(b); });
     198             : }
     199             : 
     200         116 : void PackedRTree::init(const uint16_t nodeSize)
     201             : {
     202         116 :     if (nodeSize < 2)
     203           0 :         throw std::invalid_argument("Node size must be at least 2");
     204         116 :     if (_numItems == 0)
     205           0 :         throw std::invalid_argument("Cannot create empty tree");
     206         232 :     _nodeSize = std::min(std::max(nodeSize, static_cast<uint16_t>(2)),
     207         116 :                          static_cast<uint16_t>(65535));
     208         116 :     _levelBounds = generateLevelBounds(_numItems, _nodeSize);
     209         116 :     _numNodes = _levelBounds.front().second;
     210         116 :     _nodeItems = new NodeItem[static_cast<size_t>(_numNodes)];
     211         116 : }
     212             : 
     213             : std::vector<std::pair<uint64_t, uint64_t>>
     214         158 : PackedRTree::generateLevelBounds(const uint64_t numItems,
     215             :                                  const uint16_t nodeSize)
     216             : {
     217         158 :     if (nodeSize < 2)
     218           0 :         throw std::invalid_argument("Node size must be at least 2");
     219         158 :     if (numItems == 0)
     220           0 :         throw std::invalid_argument("Number of items must be greater than 0");
     221         158 :     if (numItems >
     222         158 :         std::numeric_limits<uint64_t>::max() - ((numItems / nodeSize) * 2))
     223           0 :         throw std::overflow_error("Number of items too large");
     224             : 
     225             :     // number of nodes per level in bottom-up order
     226         316 :     std::vector<uint64_t> levelNumNodes;
     227         158 :     uint64_t n = numItems;
     228         158 :     uint64_t numNodes = n;
     229         158 :     levelNumNodes.push_back(n);
     230           0 :     do
     231             :     {
     232         158 :         n = (n + nodeSize - 1) / nodeSize;
     233         158 :         numNodes += n;
     234         158 :         levelNumNodes.push_back(n);
     235         158 :     } while (n != 1);
     236             : 
     237             :     // bounds per level in reversed storage order (top-down)
     238         316 :     std::vector<uint64_t> levelOffsets;
     239         158 :     n = numNodes;
     240         474 :     for (auto size : levelNumNodes)
     241         316 :         levelOffsets.push_back(n -= size);
     242         158 :     std::vector<std::pair<uint64_t, uint64_t>> levelBounds;
     243         474 :     for (size_t i = 0; i < levelNumNodes.size(); i++)
     244         316 :         levelBounds.push_back(std::pair<uint64_t, uint64_t>(
     245         632 :             levelOffsets[i], levelOffsets[i] + levelNumNodes[i]));
     246         316 :     return levelBounds;
     247             : }
     248             : 
     249         116 : void PackedRTree::generateNodes()
     250             : {
     251         232 :     for (uint32_t i = 0; i < _levelBounds.size() - 1; i++)
     252             :     {
     253         116 :         auto pos = _levelBounds[i].first;
     254         116 :         auto end = _levelBounds[i].second;
     255         116 :         auto newpos = _levelBounds[i + 1].first;
     256         232 :         while (pos < end)
     257             :         {
     258         116 :             NodeItem node = NodeItem::create(pos);
     259         277 :             for (uint32_t j = 0; j < _nodeSize && pos < end; j++)
     260         161 :                 node.expand(_nodeItems[pos++]);
     261         116 :             _nodeItems[newpos++] = node;
     262             :         }
     263             :     }
     264         116 : }
     265             : 
     266           0 : void PackedRTree::fromData(const void *data)
     267             : {
     268           0 :     auto buf = reinterpret_cast<const uint8_t *>(data);
     269           0 :     const NodeItem *pn = reinterpret_cast<const NodeItem *>(buf);
     270           0 :     for (uint64_t i = 0; i < _numNodes; i++)
     271             :     {
     272           0 :         NodeItem n = *pn++;
     273           0 :         _nodeItems[i] = n;
     274           0 :         _extent.expand(n);
     275             :     }
     276           0 : }
     277             : 
     278           0 : PackedRTree::PackedRTree(const std::vector<std::shared_ptr<Item>> &items,
     279           0 :                          const NodeItem &extent, const uint16_t nodeSize)
     280           0 :     : _extent(extent), _numItems(items.size())
     281             : {
     282           0 :     init(nodeSize);
     283           0 :     for (size_t i = 0; i < _numItems; i++)
     284           0 :         _nodeItems[_numNodes - _numItems + i] = items[i]->nodeItem;
     285           0 :     generateNodes();
     286           0 : }
     287             : 
     288           0 : PackedRTree::PackedRTree(const std::vector<NodeItem> &nodes,
     289           0 :                          const NodeItem &extent, const uint16_t nodeSize)
     290           0 :     : _extent(extent), _numItems(nodes.size())
     291             : {
     292           0 :     init(nodeSize);
     293           0 :     for (size_t i = 0; i < _numItems; i++)
     294           0 :         _nodeItems[_numNodes - _numItems + i] = nodes[i];
     295           0 :     generateNodes();
     296           0 : }
     297             : 
     298           0 : PackedRTree::PackedRTree(const void *data, const uint64_t numItems,
     299           0 :                          const uint16_t nodeSize)
     300           0 :     : _extent(NodeItem::create(0)), _numItems(numItems)
     301             : {
     302           0 :     init(nodeSize);
     303           0 :     fromData(data);
     304           0 : }
     305             : 
     306         116 : PackedRTree::PackedRTree(std::function<void(NodeItem *)> fillNodeItems,
     307             :                          const uint64_t numItems, const NodeItem &extent,
     308         116 :                          const uint16_t nodeSize)
     309         116 :     : _extent(extent), _numItems(numItems)
     310             : {
     311         116 :     init(nodeSize);
     312         116 :     fillNodeItems(_nodeItems + _numNodes - _numItems);
     313         116 :     generateNodes();
     314         116 : }
     315             : 
     316             : std::vector<SearchResultItem>
     317           0 : PackedRTree::search(double minX, double minY, double maxX, double maxY) const
     318             : {
     319           0 :     uint64_t leafNodesOffset = _levelBounds.front().first;
     320           0 :     NodeItem n{minX, minY, maxX, maxY, 0};
     321           0 :     std::vector<SearchResultItem> results;
     322           0 :     std::unordered_map<uint64_t, uint64_t> queue;
     323           0 :     queue.insert(std::pair<uint64_t, uint64_t>(0, _levelBounds.size() - 1));
     324           0 :     while (queue.size() != 0)
     325             :     {
     326           0 :         auto next = queue.begin();
     327           0 :         uint64_t nodeIndex = next->first;
     328           0 :         uint64_t level = next->second;
     329           0 :         queue.erase(next);
     330           0 :         bool isLeafNode = nodeIndex >= _numNodes - _numItems;
     331             :         // find the end index of the node
     332             :         uint64_t end =
     333           0 :             std::min(static_cast<uint64_t>(nodeIndex + _nodeSize),
     334           0 :                      _levelBounds[static_cast<size_t>(level)].second);
     335             :         // search through child nodes
     336           0 :         for (uint64_t pos = nodeIndex; pos < end; pos++)
     337             :         {
     338           0 :             const auto &nodeItem = _nodeItems[static_cast<size_t>(pos)];
     339           0 :             if (!n.intersects(nodeItem))
     340           0 :                 continue;
     341           0 :             if (isLeafNode)
     342           0 :                 results.push_back({nodeItem.offset, pos - leafNodesOffset});
     343             :             else
     344             :                 queue.insert(
     345           0 :                     std::pair<uint64_t, uint64_t>(nodeItem.offset, level - 1));
     346             :         }
     347             :     }
     348           0 :     return results;
     349             : }
     350             : 
     351          33 : std::vector<SearchResultItem> PackedRTree::streamSearch(
     352             :     const uint64_t numItems, const uint16_t nodeSize, const NodeItem &item,
     353             :     const std::function<void(uint8_t *, size_t, size_t)> &readNode)
     354             : {
     355          66 :     auto levelBounds = generateLevelBounds(numItems, nodeSize);
     356          33 :     uint64_t leafNodesOffset = levelBounds.front().first;
     357          33 :     uint64_t numNodes = levelBounds.front().second;
     358          66 :     auto nodeItems = std::vector<NodeItem>(nodeSize);
     359          33 :     uint8_t *nodesBuf = reinterpret_cast<uint8_t *>(nodeItems.data());
     360             :     // use ordered search queue to make index traversal in sequential order
     361          66 :     std::map<uint64_t, uint64_t> queue;
     362          33 :     std::vector<SearchResultItem> results;
     363          33 :     queue.insert(std::pair<uint64_t, uint64_t>(0, levelBounds.size() - 1));
     364          94 :     while (queue.size() != 0)
     365             :     {
     366          61 :         auto next = queue.begin();
     367          61 :         uint64_t nodeIndex = next->first;
     368          61 :         uint64_t level = next->second;
     369          61 :         queue.erase(next);
     370          61 :         bool isLeafNode = nodeIndex >= numNodes - numItems;
     371             :         // find the end index of the node
     372         122 :         uint64_t end = std::min(static_cast<uint64_t>(nodeIndex + nodeSize),
     373          61 :                                 levelBounds[static_cast<size_t>(level)].second);
     374          61 :         uint64_t length = end - nodeIndex;
     375          61 :         readNode(nodesBuf, static_cast<size_t>(nodeIndex * sizeof(NodeItem)),
     376          61 :                  static_cast<size_t>(length * sizeof(NodeItem)));
     377             : #if !CPL_IS_LSB
     378             :         for (size_t i = 0; i < static_cast<size_t>(length); i++)
     379             :         {
     380             :             CPL_LSBPTR64(&nodeItems[i].minX);
     381             :             CPL_LSBPTR64(&nodeItems[i].minY);
     382             :             CPL_LSBPTR64(&nodeItems[i].maxX);
     383             :             CPL_LSBPTR64(&nodeItems[i].maxY);
     384             :             CPL_LSBPTR64(&nodeItems[i].offset);
     385             :         }
     386             : #endif
     387             :         // search through child nodes
     388         270 :         for (uint64_t pos = nodeIndex; pos < end; pos++)
     389             :         {
     390         209 :             uint64_t nodePos = pos - nodeIndex;
     391         209 :             const auto &nodeItem = nodeItems[static_cast<size_t>(nodePos)];
     392         209 :             if (!item.intersects(nodeItem))
     393          56 :                 continue;
     394         153 :             if (isLeafNode)
     395         125 :                 results.push_back({nodeItem.offset, pos - leafNodesOffset});
     396             :             else
     397             :                 queue.insert(
     398          28 :                     std::pair<uint64_t, uint64_t>(nodeItem.offset, level - 1));
     399             :         }
     400             :     }
     401          66 :     return results;
     402             : }
     403             : 
     404           0 : uint64_t PackedRTree::size() const
     405             : {
     406           0 :     return _numNodes * sizeof(NodeItem);
     407             : }
     408             : 
     409         164 : uint64_t PackedRTree::size(const uint64_t numItems, const uint16_t nodeSize)
     410             : {
     411         164 :     if (nodeSize < 2)
     412           0 :         throw std::invalid_argument("Node size must be at least 2");
     413         164 :     if (numItems == 0)
     414           0 :         throw std::invalid_argument("Number of items must be greater than 0");
     415             :     const uint16_t nodeSizeMin =
     416         328 :         std::min(std::max(nodeSize, static_cast<uint16_t>(2)),
     417         164 :                  static_cast<uint16_t>(65535));
     418             :     // limit so that resulting size in bytes can be represented by uint64_t
     419         164 :     if (numItems > static_cast<uint64_t>(1) << 56)
     420           0 :         throw std::overflow_error("Number of items must be less than 2^56");
     421         164 :     uint64_t n = numItems;
     422         164 :     uint64_t numNodes = n;
     423           0 :     do
     424             :     {
     425         164 :         n = (n + nodeSizeMin - 1) / nodeSizeMin;
     426         164 :         numNodes += n;
     427         164 :     } while (n != 1);
     428         164 :     return numNodes * sizeof(NodeItem);
     429             : }
     430             : 
     431         116 : void PackedRTree::streamWrite(
     432             :     const std::function<void(uint8_t *, size_t)> &writeData)
     433             : {
     434             : #if !CPL_IS_LSB
     435             :     for (size_t i = 0; i < static_cast<size_t>(_numNodes); i++)
     436             :     {
     437             :         CPL_LSBPTR64(&_nodeItems[i].minX);
     438             :         CPL_LSBPTR64(&_nodeItems[i].minY);
     439             :         CPL_LSBPTR64(&_nodeItems[i].maxX);
     440             :         CPL_LSBPTR64(&_nodeItems[i].maxY);
     441             :         CPL_LSBPTR64(&_nodeItems[i].offset);
     442             :     }
     443             : #endif
     444         116 :     writeData(reinterpret_cast<uint8_t *>(_nodeItems),
     445         116 :               static_cast<size_t>(_numNodes * sizeof(NodeItem)));
     446             : #if !CPL_IS_LSB
     447             :     for (size_t i = 0; i < static_cast<size_t>(_numNodes); i++)
     448             :     {
     449             :         CPL_LSBPTR64(&_nodeItems[i].minX);
     450             :         CPL_LSBPTR64(&_nodeItems[i].minY);
     451             :         CPL_LSBPTR64(&_nodeItems[i].maxX);
     452             :         CPL_LSBPTR64(&_nodeItems[i].maxY);
     453             :         CPL_LSBPTR64(&_nodeItems[i].offset);
     454             :     }
     455             : #endif
     456         116 : }
     457             : 
     458           0 : NodeItem PackedRTree::getExtent() const
     459             : {
     460           0 :     return _extent;
     461             : }
     462             : 
     463             : }  // namespace FlatGeobuf