From 0f27c4467882c581960614550b4c31a96d2b3c13 Mon Sep 17 00:00:00 2001 From: TANG ZhiXiong Date: Fri, 8 Sep 2023 23:14:37 +0800 Subject: [PATCH] use header-only version of packedrtree; test logging (#10) * parallel stl, release gil * logging level # head: (): * ostream redirect * logging not totally okay * not good * can capture * not good * okay * packed rtree * update packedrtree, header-only version --------- Co-authored-by: TANG ZHIXIONG --- 3rdparty/packedrtree.cpp | 481 -------------------------- 3rdparty/packedrtree.h | 182 ---------- 3rdparty/packedrtree.hpp | 419 ++++++++++++++++++++++ CMakeLists.txt | 1 + src/bindings/pybind11_network.cpp | 17 +- src/bindings/pybind11_packedrtree.cpp | 3 +- src/bindings/utils.cpp | 35 ++ src/nano_fmm/network.cpp | 17 +- src/nano_fmm/network.hpp | 7 +- src/source.cpp | 1 - tests/test_basic.py | 72 +++- 11 files changed, 554 insertions(+), 681 deletions(-) delete mode 100644 3rdparty/packedrtree.cpp delete mode 100644 3rdparty/packedrtree.h create mode 100644 3rdparty/packedrtree.hpp diff --git a/3rdparty/packedrtree.cpp b/3rdparty/packedrtree.cpp deleted file mode 100644 index d41993f..0000000 --- a/3rdparty/packedrtree.cpp +++ /dev/null @@ -1,481 +0,0 @@ -/****************************************************************************** - * - * Project: FlatGeobuf - * Purpose: Packed RTree management - * Author: Björn Harrtell - * - ****************************************************************************** - * Copyright (c) 2018-2020, Björn Harrtell - * - * Permission is hereby granted, free of charge, to any person obtaining a - * copy of this software and associated documentation files (the "Software"), - * to deal in the Software without restriction, including without limitation - * the rights to use, copy, modify, merge, publish, distribute, sublicense, - * and/or sell copies of the Software, and to permit persons to whom the - * Software is furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included - * in all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS - * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL - * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING - * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER - * DEALINGS IN THE SOFTWARE. - ****************************************************************************/ - -// NOTE: The upstream of this file is in -// https://github.com/bjornharrtell/flatgeobuf/tree/master/src/cpp - -#ifdef GDAL_COMPILATION -#include "cpl_port.h" -#else -#define CPL_IS_LSB 1 -#endif - -#include "packedrtree.h" - -#include -#include -#include -#include -#include - -namespace FlatGeobuf -{ - -const NodeItem &NodeItem::expand(const NodeItem &r) -{ - if (r.minX < minX) - minX = r.minX; - if (r.minY < minY) - minY = r.minY; - if (r.maxX > maxX) - maxX = r.maxX; - if (r.maxY > maxY) - maxY = r.maxY; - return *this; -} - -NodeItem NodeItem::create(uint64_t offset) -{ - return {std::numeric_limits::infinity(), - std::numeric_limits::infinity(), - -1 * std::numeric_limits::infinity(), - -1 * std::numeric_limits::infinity(), offset}; -} - -bool NodeItem::intersects(const NodeItem &r) const -{ - if (maxX < r.minX) - return false; - if (maxY < r.minY) - return false; - if (minX > r.maxX) - return false; - if (minY > r.maxY) - return false; - return true; -} - -std::vector NodeItem::toVector() -{ - return std::vector{minX, minY, maxX, maxY}; -} - -// Based on public domain code at -// https://github.com/rawrunprotected/hilbert_curves -uint32_t hilbert(uint32_t x, uint32_t y) -{ - uint32_t a = x ^ y; - uint32_t b = 0xFFFF ^ a; - uint32_t c = 0xFFFF ^ (x | y); - uint32_t d = x & (y ^ 0xFFFF); - - uint32_t A = a | (b >> 1); - uint32_t B = (a >> 1) ^ a; - uint32_t C = ((c >> 1) ^ (b & (d >> 1))) ^ c; - uint32_t D = ((a & (c >> 1)) ^ (d >> 1)) ^ d; - - a = A; - b = B; - c = C; - d = D; - A = ((a & (a >> 2)) ^ (b & (b >> 2))); - B = ((a & (b >> 2)) ^ (b & ((a ^ b) >> 2))); - C ^= ((a & (c >> 2)) ^ (b & (d >> 2))); - D ^= ((b & (c >> 2)) ^ ((a ^ b) & (d >> 2))); - - a = A; - b = B; - c = C; - d = D; - A = ((a & (a >> 4)) ^ (b & (b >> 4))); - B = ((a & (b >> 4)) ^ (b & ((a ^ b) >> 4))); - C ^= ((a & (c >> 4)) ^ (b & (d >> 4))); - D ^= ((b & (c >> 4)) ^ ((a ^ b) & (d >> 4))); - - a = A; - b = B; - c = C; - d = D; - C ^= ((a & (c >> 8)) ^ (b & (d >> 8))); - D ^= ((b & (c >> 8)) ^ ((a ^ b) & (d >> 8))); - - a = C ^ (C >> 1); - b = D ^ (D >> 1); - - uint32_t i0 = x ^ y; - uint32_t i1 = b | (0xFFFF ^ (i0 | a)); - - i0 = (i0 | (i0 << 8)) & 0x00FF00FF; - i0 = (i0 | (i0 << 4)) & 0x0F0F0F0F; - i0 = (i0 | (i0 << 2)) & 0x33333333; - i0 = (i0 | (i0 << 1)) & 0x55555555; - - i1 = (i1 | (i1 << 8)) & 0x00FF00FF; - i1 = (i1 | (i1 << 4)) & 0x0F0F0F0F; - i1 = (i1 | (i1 << 2)) & 0x33333333; - i1 = (i1 | (i1 << 1)) & 0x55555555; - - uint32_t value = ((i1 << 1) | i0); - - return value; -} - -uint32_t hilbert(const NodeItem &r, uint32_t hilbertMax, const double minX, - const double minY, const double width, const double height) -{ - uint32_t x = 0; - uint32_t y = 0; - uint32_t v; - if (width != 0.0) - x = static_cast( - floor(hilbertMax * ((r.minX + r.maxX) / 2 - minX) / width)); - if (height != 0.0) - y = static_cast( - floor(hilbertMax * ((r.minY + r.maxY) / 2 - minY) / height)); - v = hilbert(x, y); - return v; -} - -void hilbertSort(std::vector> &items) -{ - NodeItem extent = calcExtent(items); - const double minX = extent.minX; - const double minY = extent.minY; - const double width = extent.width(); - const double height = extent.height(); - std::sort(items.begin(), items.end(), - [minX, minY, width, height](std::shared_ptr a, - std::shared_ptr b) - { - uint32_t ha = hilbert(a->nodeItem, HILBERT_MAX, minX, minY, - width, height); - uint32_t hb = hilbert(b->nodeItem, HILBERT_MAX, minX, minY, - width, height); - return ha > hb; - }); -} - -void hilbertSort(std::vector &items) { - hilbertSort(items, calcExtent(items)); -} - -void hilbertSort(std::vector &items, const NodeItem &extent) -{ - const double minX = extent.minX; - const double minY = extent.minY; - const double width = extent.width(); - const double height = extent.height(); - std::sort(items.begin(), items.end(), - [minX, minY, width, height](const NodeItem &a, const NodeItem &b) - { - uint32_t ha = - hilbert(a, HILBERT_MAX, minX, minY, width, height); - uint32_t hb = - hilbert(b, HILBERT_MAX, minX, minY, width, height); - return ha > hb; - }); -} - -NodeItem calcExtent(const std::vector> &items) -{ - return std::accumulate(items.begin(), items.end(), NodeItem::create(0), - [](NodeItem a, const std::shared_ptr &b) - { return a.expand(b->nodeItem); }); -} - -NodeItem calcExtent(const std::vector &nodes) -{ - return std::accumulate(nodes.begin(), nodes.end(), NodeItem::create(0), - [](NodeItem a, const NodeItem &b) - { return a.expand(b); }); -} - -void PackedRTree::init(const uint16_t nodeSize) -{ - if (nodeSize < 2) - throw std::invalid_argument("Node size must be at least 2"); - if (_numItems == 0) - throw std::invalid_argument("Cannot create empty tree"); - _nodeSize = std::min(std::max(nodeSize, static_cast(2)), - static_cast(65535)); - _levelBounds = generateLevelBounds(_numItems, _nodeSize); - _numNodes = _levelBounds.front().second; - _nodeItems = new NodeItem[static_cast(_numNodes)]; -} - -std::vector> -PackedRTree::generateLevelBounds(const uint64_t numItems, - const uint16_t nodeSize) -{ - if (nodeSize < 2) - throw std::invalid_argument("Node size must be at least 2"); - if (numItems == 0) - throw std::invalid_argument("Number of items must be greater than 0"); - if (numItems > - std::numeric_limits::max() - ((numItems / nodeSize) * 2)) - throw std::overflow_error("Number of items too large"); - - // number of nodes per level in bottom-up order - std::vector levelNumNodes; - uint64_t n = numItems; - uint64_t numNodes = n; - levelNumNodes.push_back(n); - do - { - n = (n + nodeSize - 1) / nodeSize; - numNodes += n; - levelNumNodes.push_back(n); - } while (n != 1); - - // bounds per level in reversed storage order (top-down) - std::vector levelOffsets; - n = numNodes; - for (auto size : levelNumNodes) - levelOffsets.push_back(n -= size); - std::vector> levelBounds; - for (size_t i = 0; i < levelNumNodes.size(); i++) - levelBounds.push_back(std::pair( - levelOffsets[i], levelOffsets[i] + levelNumNodes[i])); - return levelBounds; -} - -void PackedRTree::generateNodes() -{ - for (uint32_t i = 0; i < _levelBounds.size() - 1; i++) - { - auto pos = _levelBounds[i].first; - auto end = _levelBounds[i].second; - auto newpos = _levelBounds[i + 1].first; - while (pos < end) - { - NodeItem node = NodeItem::create(pos); - for (uint32_t j = 0; j < _nodeSize && pos < end; j++) - node.expand(_nodeItems[pos++]); - _nodeItems[newpos++] = node; - } - } -} - -void PackedRTree::fromData(const void *data) -{ - auto buf = reinterpret_cast(data); - const NodeItem *pn = reinterpret_cast(buf); - for (uint64_t i = 0; i < _numNodes; i++) - { - NodeItem n = *pn++; - _nodeItems[i] = n; - _extent.expand(n); - } -} - -PackedRTree::PackedRTree(const std::vector> &items, - const NodeItem &extent, const uint16_t nodeSize) - : _extent(extent), _numItems(items.size()) -{ - init(nodeSize); - for (size_t i = 0; i < _numItems; i++) - _nodeItems[_numNodes - _numItems + i] = items[i]->nodeItem; - generateNodes(); -} - -PackedRTree::PackedRTree(const std::vector &nodes, - const NodeItem &extent, const uint16_t nodeSize) - : _extent(extent), _numItems(nodes.size()) -{ - init(nodeSize); - for (size_t i = 0; i < _numItems; i++) - _nodeItems[_numNodes - _numItems + i] = nodes[i]; - generateNodes(); -} - -PackedRTree::PackedRTree(const void *data, const uint64_t numItems, - const uint16_t nodeSize) - : _extent(NodeItem::create(0)), _numItems(numItems) -{ - init(nodeSize); - fromData(data); -} - -PackedRTree::PackedRTree(std::function fillNodeItems, - const uint64_t numItems, const NodeItem &extent, - const uint16_t nodeSize) - : _extent(extent), _numItems(numItems) -{ - init(nodeSize); - fillNodeItems(_nodeItems + _numNodes - _numItems); - generateNodes(); -} - -std::vector -PackedRTree::search(double minX, double minY, double maxX, double maxY) const -{ - uint64_t leafNodesOffset = _levelBounds.front().first; - NodeItem n{minX, minY, maxX, maxY, 0}; - std::vector results; - std::unordered_map queue; - queue.insert(std::pair(0, _levelBounds.size() - 1)); - while (queue.size() != 0) - { - auto next = queue.begin(); - uint64_t nodeIndex = next->first; - uint64_t level = next->second; - queue.erase(next); - bool isLeafNode = nodeIndex >= _numNodes - _numItems; - // find the end index of the node - uint64_t end = - std::min(static_cast(nodeIndex + _nodeSize), - _levelBounds[static_cast(level)].second); - // search through child nodes - for (uint64_t pos = nodeIndex; pos < end; pos++) - { - auto nodeItem = _nodeItems[static_cast(pos)]; - if (!n.intersects(nodeItem)) - continue; - if (isLeafNode) - results.push_back({nodeItem.offset, pos - leafNodesOffset}); - else - queue.insert( - std::pair(nodeItem.offset, level - 1)); - } - } - return results; -} - -std::vector PackedRTree::streamSearch( - const uint64_t numItems, const uint16_t nodeSize, const NodeItem &item, - const std::function &readNode) -{ - auto levelBounds = generateLevelBounds(numItems, nodeSize); - uint64_t leafNodesOffset = levelBounds.front().first; - uint64_t numNodes = levelBounds.front().second; - auto nodeItems = std::vector(nodeSize); - uint8_t *nodesBuf = reinterpret_cast(nodeItems.data()); - // use ordered search queue to make index traversal in sequential order - std::map queue; - std::vector results; - queue.insert(std::pair(0, levelBounds.size() - 1)); - while (queue.size() != 0) - { - auto next = queue.begin(); - uint64_t nodeIndex = next->first; - uint64_t level = next->second; - queue.erase(next); - bool isLeafNode = nodeIndex >= numNodes - numItems; - // find the end index of the node - uint64_t end = std::min(static_cast(nodeIndex + nodeSize), - levelBounds[static_cast(level)].second); - uint64_t length = end - nodeIndex; - readNode(nodesBuf, static_cast(nodeIndex * sizeof(NodeItem)), - static_cast(length * sizeof(NodeItem))); -#if !CPL_IS_LSB - for (size_t i = 0; i < static_cast(length); i++) - { - CPL_LSBPTR64(&nodeItems[i].minX); - CPL_LSBPTR64(&nodeItems[i].minY); - CPL_LSBPTR64(&nodeItems[i].maxX); - CPL_LSBPTR64(&nodeItems[i].maxY); - CPL_LSBPTR64(&nodeItems[i].offset); - } -#endif - // search through child nodes - for (uint64_t pos = nodeIndex; pos < end; pos++) - { - uint64_t nodePos = pos - nodeIndex; - auto nodeItem = nodeItems[static_cast(nodePos)]; - if (!item.intersects(nodeItem)) - continue; - if (isLeafNode) - results.push_back({nodeItem.offset, pos - leafNodesOffset}); - else - queue.insert( - std::pair(nodeItem.offset, level - 1)); - } - } - return results; -} - -uint64_t PackedRTree::size() const -{ - return _numNodes * sizeof(NodeItem); -} - -uint64_t PackedRTree::size(const uint64_t numItems, const uint16_t nodeSize) -{ - if (nodeSize < 2) - throw std::invalid_argument("Node size must be at least 2"); - if (numItems == 0) - throw std::invalid_argument("Number of items must be greater than 0"); - const uint16_t nodeSizeMin = - std::min(std::max(nodeSize, static_cast(2)), - static_cast(65535)); - // limit so that resulting size in bytes can be represented by uint64_t - if (numItems > static_cast(1) << 56) - throw std::overflow_error("Number of items must be less than 2^56"); - uint64_t n = numItems; - uint64_t numNodes = n; - do - { - n = (n + nodeSizeMin - 1) / nodeSizeMin; - numNodes += n; - } while (n != 1); - return numNodes * sizeof(NodeItem); -} - -void PackedRTree::streamWrite( - const std::function &writeData) -{ -#if !CPL_IS_LSB - for (size_t i = 0; i < static_cast(_numNodes); i++) - { - CPL_LSBPTR64(&_nodeItems[i].minX); - CPL_LSBPTR64(&_nodeItems[i].minY); - CPL_LSBPTR64(&_nodeItems[i].maxX); - CPL_LSBPTR64(&_nodeItems[i].maxY); - CPL_LSBPTR64(&_nodeItems[i].offset); - } -#endif - writeData(reinterpret_cast(_nodeItems), - static_cast(_numNodes * sizeof(NodeItem))); -#if !CPL_IS_LSB - for (size_t i = 0; i < static_cast(_numNodes); i++) - { - CPL_LSBPTR64(&_nodeItems[i].minX); - CPL_LSBPTR64(&_nodeItems[i].minY); - CPL_LSBPTR64(&_nodeItems[i].maxX); - CPL_LSBPTR64(&_nodeItems[i].maxY); - CPL_LSBPTR64(&_nodeItems[i].offset); - } -#endif -} - -NodeItem PackedRTree::getExtent() const -{ - return _extent; -} - -} // namespace FlatGeobuf diff --git a/3rdparty/packedrtree.h b/3rdparty/packedrtree.h deleted file mode 100644 index 9a09e2a..0000000 --- a/3rdparty/packedrtree.h +++ /dev/null @@ -1,182 +0,0 @@ -/****************************************************************************** - * - * Project: FlatGeobuf - * Purpose: Packed RTree management - * Author: Björn Harrtell - * - ****************************************************************************** - * Copyright (c) 2018-2020, Björn Harrtell - * - * Permission is hereby granted, free of charge, to any person obtaining a - * copy of this software and associated documentation files (the "Software"), - * to deal in the Software without restriction, including without limitation - * the rights to use, copy, modify, merge, publish, distribute, sublicense, - * and/or sell copies of the Software, and to permit persons to whom the - * Software is furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included - * in all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS - * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL - * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING - * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER - * DEALINGS IN THE SOFTWARE. - ****************************************************************************/ - -// NOTE: The upstream of this file is in -// https://github.com/bjornharrtell/flatgeobuf/tree/master/src/cpp - -#ifndef FLATGEOBUF_PACKEDRTREE_H_ -#define FLATGEOBUF_PACKEDRTREE_H_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace FlatGeobuf -{ - -struct NodeItem -{ - double minX; - double minY; - double maxX; - double maxY; - uint64_t offset; - double width() const - { - return maxX - minX; - } - double height() const - { - return maxY - minY; - } - static NodeItem sum(NodeItem a, const NodeItem &b) - { - a.expand(b); - return a; - } - static NodeItem create(uint64_t offset = 0); - const NodeItem &expand(const NodeItem &r); - bool intersects(const NodeItem &r) const; - std::vector toVector(); -}; - -inline bool operator==(const NodeItem& lhs, const NodeItem& rhs) -{ - return lhs.minX == rhs.minX && lhs.minY == rhs.minY && lhs.maxX == rhs.maxX && lhs.maxY == rhs.maxY && lhs.offset == rhs.offset; -} - -struct Item -{ - NodeItem nodeItem; -}; - -struct SearchResultItem -{ - uint64_t offset; - uint64_t index; -}; - -inline bool operator==(const SearchResultItem& lhs, const SearchResultItem& rhs) -{ - return lhs.index == rhs.index && lhs.offset == rhs.offset; -} - -std::ostream &operator<<(std::ostream &os, NodeItem const &value); - -uint32_t hilbert(uint32_t x, uint32_t y); -uint32_t hilbert(const NodeItem &n, uint32_t hilbertMax, const double minX, - const double minY, const double width, const double height); -void hilbertSort(std::vector> &items); - -constexpr uint32_t HILBERT_MAX = (1 << 16) - 1; - -template -NodeItem calcExtent(const std::deque &items) -{ - return std::accumulate(items.begin(), items.end(), NodeItem::create(0), - [](NodeItem a, const ITEM_TYPE &b) - { return a.expand(b.nodeItem); }); -} - -template void hilbertSort(std::deque &items) -{ - NodeItem extent = calcExtent(items); - const double minX = extent.minX; - const double minY = extent.minY; - const double width = extent.width(); - const double height = extent.height(); - std::sort( - items.begin(), items.end(), - [minX, minY, width, height](const ITEM_TYPE &a, const ITEM_TYPE &b) - { - uint32_t ha = - hilbert(a.nodeItem, HILBERT_MAX, minX, minY, width, height); - uint32_t hb = - hilbert(b.nodeItem, HILBERT_MAX, minX, minY, width, height); - return ha > hb; - }); -} - -void hilbertSort(std::vector &items); -void hilbertSort(std::vector &items, const NodeItem &extent); -NodeItem calcExtent(const std::vector> &items); -NodeItem calcExtent(const std::vector &rects); - -/** - * Packed R-Tree - * Based on https://github.com/mourner/flatbush - */ -class PackedRTree -{ - NodeItem _extent; - NodeItem *_nodeItems = nullptr; - uint64_t _numItems; - uint64_t _numNodes; - uint16_t _nodeSize; - std::vector> _levelBounds; - void init(const uint16_t nodeSize); - void generateNodes(); - void fromData(const void *data); - - public: - ~PackedRTree() - { - if (_nodeItems != nullptr) - delete[] _nodeItems; - } - PackedRTree(const std::vector> &items, - const NodeItem &extent, const uint16_t nodeSize = 16); - PackedRTree(const std::vector &nodes, const NodeItem &extent, - const uint16_t nodeSize = 16); - PackedRTree(const void *data, const uint64_t numItems, - const uint16_t nodeSize = 16); - PackedRTree(std::function fillNodeItems, - const uint64_t numItems, const NodeItem &extent, - const uint16_t nodeSize = 16); - std::vector search(double minX, double minY, double maxX, - double maxY) const; - static std::vector streamSearch( - const uint64_t numItems, const uint16_t nodeSize, const NodeItem &item, - const std::function &readNode); - static std::vector> - generateLevelBounds(const uint64_t numItems, const uint16_t nodeSize); - uint64_t size() const; - static uint64_t size(const uint64_t numItems, const uint16_t nodeSize = 16); - NodeItem getExtent() const; - void streamWrite(const std::function &writeData); -}; - -} // namespace FlatGeobuf - -#endif diff --git a/3rdparty/packedrtree.hpp b/3rdparty/packedrtree.hpp new file mode 100644 index 0000000..05180c7 --- /dev/null +++ b/3rdparty/packedrtree.hpp @@ -0,0 +1,419 @@ +// sync with +// https://github.com/cubao/headers/blob/main/include/packedrtree.hpp + +/****************************************************************************** + * + * Project: FlatGeobuf + * Purpose: Packed RTree management + * Author: Björn Harrtell + * + ****************************************************************************** + * Copyright (c) 2018-2020, Björn Harrtell + * + * Permission is hereby granted, free of charge, to any person obtaining a + * copy of this software and associated documentation files (the "Software"), + * to deal in the Software without restriction, including without limitation + * the rights to use, copy, modify, merge, publish, distribute, sublicense, + * and/or sell copies of the Software, and to permit persons to whom the + * Software is furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included + * in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL + * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER + * DEALINGS IN THE SOFTWARE. + ****************************************************************************/ + +// NOTE: The upstream of this file is in +// https://github.com/bjornharrtell/flatgeobuf/tree/master/src/cpp + +#ifndef FLATGEOBUF_PACKEDRTREE_H_ +#define FLATGEOBUF_PACKEDRTREE_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace FlatGeobuf +{ + +struct NodeItem +{ + double minX; + double minY; + double maxX; + double maxY; + uint64_t offset; + double width() const { return maxX - minX; } + double height() const { return maxY - minY; } + static NodeItem sum(NodeItem a, const NodeItem &b) + { + a.expand(b); + return a; + } + static NodeItem create(uint64_t offset = 0) + { + return {std::numeric_limits::infinity(), + std::numeric_limits::infinity(), + -1 * std::numeric_limits::infinity(), + -1 * std::numeric_limits::infinity(), offset}; + } + const NodeItem &expand(const NodeItem &r) + { + if (r.minX < minX) + minX = r.minX; + if (r.minY < minY) + minY = r.minY; + if (r.maxX > maxX) + maxX = r.maxX; + if (r.maxY > maxY) + maxY = r.maxY; + return *this; + } + bool intersects(const NodeItem &r) const + { + if (maxX < r.minX) + return false; + if (maxY < r.minY) + return false; + if (minX > r.maxX) + return false; + if (minY > r.maxY) + return false; + return true; + } +}; + +inline bool operator==(const NodeItem &lhs, const NodeItem &rhs) +{ + return lhs.minX == rhs.minX && lhs.minY == rhs.minY && + lhs.maxX == rhs.maxX && lhs.maxY == rhs.maxY && + lhs.offset == rhs.offset; +} + +struct Item +{ + NodeItem nodeItem; +}; + +struct SearchResultItem +{ + uint64_t offset; + uint64_t index; +}; + +inline bool operator==(const SearchResultItem &lhs, const SearchResultItem &rhs) +{ + return lhs.index == rhs.index && lhs.offset == rhs.offset; +} + +// Based on public domain code at +// https://github.com/rawrunprotected/hilbert_curves +inline uint32_t hilbert(uint32_t x, uint32_t y) +{ + uint32_t a = x ^ y; + uint32_t b = 0xFFFF ^ a; + uint32_t c = 0xFFFF ^ (x | y); + uint32_t d = x & (y ^ 0xFFFF); + + uint32_t A = a | (b >> 1); + uint32_t B = (a >> 1) ^ a; + uint32_t C = ((c >> 1) ^ (b & (d >> 1))) ^ c; + uint32_t D = ((a & (c >> 1)) ^ (d >> 1)) ^ d; + + a = A; + b = B; + c = C; + d = D; + A = ((a & (a >> 2)) ^ (b & (b >> 2))); + B = ((a & (b >> 2)) ^ (b & ((a ^ b) >> 2))); + C ^= ((a & (c >> 2)) ^ (b & (d >> 2))); + D ^= ((b & (c >> 2)) ^ ((a ^ b) & (d >> 2))); + + a = A; + b = B; + c = C; + d = D; + A = ((a & (a >> 4)) ^ (b & (b >> 4))); + B = ((a & (b >> 4)) ^ (b & ((a ^ b) >> 4))); + C ^= ((a & (c >> 4)) ^ (b & (d >> 4))); + D ^= ((b & (c >> 4)) ^ ((a ^ b) & (d >> 4))); + + a = A; + b = B; + c = C; + d = D; + C ^= ((a & (c >> 8)) ^ (b & (d >> 8))); + D ^= ((b & (c >> 8)) ^ ((a ^ b) & (d >> 8))); + + a = C ^ (C >> 1); + b = D ^ (D >> 1); + + uint32_t i0 = x ^ y; + uint32_t i1 = b | (0xFFFF ^ (i0 | a)); + + i0 = (i0 | (i0 << 8)) & 0x00FF00FF; + i0 = (i0 | (i0 << 4)) & 0x0F0F0F0F; + i0 = (i0 | (i0 << 2)) & 0x33333333; + i0 = (i0 | (i0 << 1)) & 0x55555555; + + i1 = (i1 | (i1 << 8)) & 0x00FF00FF; + i1 = (i1 | (i1 << 4)) & 0x0F0F0F0F; + i1 = (i1 | (i1 << 2)) & 0x33333333; + i1 = (i1 | (i1 << 1)) & 0x55555555; + + uint32_t value = ((i1 << 1) | i0); + + return value; +} + +inline uint32_t hilbert(const NodeItem &r, uint32_t hilbertMax, + const double minX, const double minY, + const double width, const double height) +{ + uint32_t x = 0; + uint32_t y = 0; + uint32_t v; + if (width != 0.0) + x = static_cast( + floor(hilbertMax * ((r.minX + r.maxX) / 2 - minX) / width)); + if (height != 0.0) + y = static_cast( + floor(hilbertMax * ((r.minY + r.maxY) / 2 - minY) / height)); + v = hilbert(x, y); + return v; +} + +constexpr uint32_t HILBERT_MAX = (1 << 16) - 1; + +inline NodeItem calcExtent(const std::vector &nodes) +{ + return std::accumulate( + nodes.begin(), nodes.end(), NodeItem::create(0), + [](NodeItem a, const NodeItem &b) { return a.expand(b); }); +} + +template void hilbertSort(std::deque &items) +{ + NodeItem extent = calcExtent(items); + const double minX = extent.minX; + const double minY = extent.minY; + const double width = extent.width(); + const double height = extent.height(); + std::sort( + items.begin(), items.end(), + [minX, minY, width, height](const ITEM_TYPE &a, const ITEM_TYPE &b) { + uint32_t ha = + hilbert(a.nodeItem, HILBERT_MAX, minX, minY, width, height); + uint32_t hb = + hilbert(b.nodeItem, HILBERT_MAX, minX, minY, width, height); + return ha > hb; + }); +} + +inline void hilbertSort(std::vector &items, const NodeItem &extent) +{ + const double minX = extent.minX; + const double minY = extent.minY; + const double width = extent.width(); + const double height = extent.height(); + std::sort( + items.begin(), items.end(), + [minX, minY, width, height](const NodeItem &a, const NodeItem &b) { + uint32_t ha = hilbert(a, HILBERT_MAX, minX, minY, width, height); + uint32_t hb = hilbert(b, HILBERT_MAX, minX, minY, width, height); + return ha > hb; + }); +} +inline void hilbertSort(std::vector &items) +{ + hilbertSort(items, calcExtent(items)); +} + +/** + * Packed R-Tree + * Based on https://github.com/mourner/flatbush + */ +class PackedRTree +{ + NodeItem _extent; + NodeItem *_nodeItems = nullptr; + uint64_t _numItems; + uint64_t _numNodes; + uint16_t _nodeSize; + std::vector> _levelBounds; + void init(const uint16_t nodeSize) + { + if (nodeSize < 2) + throw std::invalid_argument("Node size must be at least 2"); + if (_numItems == 0) + throw std::invalid_argument("Cannot create empty tree"); + _nodeSize = std::min(std::max(nodeSize, static_cast(2)), + static_cast(65535)); + _levelBounds = generateLevelBounds(_numItems, _nodeSize); + _numNodes = _levelBounds.front().second; + _nodeItems = new NodeItem[static_cast(_numNodes)]; + } + void generateNodes() + { + for (uint32_t i = 0; i < _levelBounds.size() - 1; i++) { + auto pos = _levelBounds[i].first; + auto end = _levelBounds[i].second; + auto newpos = _levelBounds[i + 1].first; + while (pos < end) { + NodeItem node = NodeItem::create(pos); + for (uint32_t j = 0; j < _nodeSize && pos < end; j++) + node.expand(_nodeItems[pos++]); + _nodeItems[newpos++] = node; + } + } + } + + void fromData(const void *data) + { + auto buf = reinterpret_cast(data); + const NodeItem *pn = reinterpret_cast(buf); + for (uint64_t i = 0; i < _numNodes; i++) { + NodeItem n = *pn++; + _nodeItems[i] = n; + _extent.expand(n); + } + } + + public: + ~PackedRTree() + { + if (_nodeItems != nullptr) + delete[] _nodeItems; + } + PackedRTree(const std::vector &nodes, const NodeItem &extent, + const uint16_t nodeSize = 16) + : _extent(extent), _numItems(nodes.size()) + { + init(nodeSize); + for (size_t i = 0; i < _numItems; i++) + _nodeItems[_numNodes - _numItems + i] = nodes[i]; + generateNodes(); + } + + PackedRTree(const void *data, const uint64_t numItems, + const uint16_t nodeSize) + : _extent(NodeItem::create(0)), _numItems(numItems) + { + init(nodeSize); + fromData(data); + } + + std::vector search(double minX, double minY, double maxX, + double maxY) const + { + uint64_t leafNodesOffset = _levelBounds.front().first; + NodeItem n{minX, minY, maxX, maxY, 0}; + std::vector results; + std::unordered_map queue; + queue.insert(std::pair(0, _levelBounds.size() - 1)); + while (queue.size() != 0) { + auto next = queue.begin(); + uint64_t nodeIndex = next->first; + uint64_t level = next->second; + queue.erase(next); + bool isLeafNode = nodeIndex >= _numNodes - _numItems; + // find the end index of the node + uint64_t end = + std::min(static_cast(nodeIndex + _nodeSize), + _levelBounds[static_cast(level)].second); + // search through child nodes + for (uint64_t pos = nodeIndex; pos < end; pos++) { + auto nodeItem = _nodeItems[static_cast(pos)]; + if (!n.intersects(nodeItem)) + continue; + if (isLeafNode) + results.push_back({nodeItem.offset, pos - leafNodesOffset}); + else + queue.insert(std::pair(nodeItem.offset, + level - 1)); + } + } + return results; + } + static std::vector> + generateLevelBounds(const uint64_t numItems, const uint16_t nodeSize) + { + if (nodeSize < 2) + throw std::invalid_argument("Node size must be at least 2"); + if (numItems == 0) + throw std::invalid_argument( + "Number of items must be greater than 0"); + if (numItems > + std::numeric_limits::max() - ((numItems / nodeSize) * 2)) + throw std::overflow_error("Number of items too large"); + + // number of nodes per level in bottom-up order + std::vector levelNumNodes; + uint64_t n = numItems; + uint64_t numNodes = n; + levelNumNodes.push_back(n); + do { + n = (n + nodeSize - 1) / nodeSize; + numNodes += n; + levelNumNodes.push_back(n); + } while (n != 1); + + // bounds per level in reversed storage order (top-down) + std::vector levelOffsets; + n = numNodes; + for (auto size : levelNumNodes) + levelOffsets.push_back(n -= size); + std::vector> levelBounds; + for (size_t i = 0; i < levelNumNodes.size(); i++) + levelBounds.push_back(std::pair( + levelOffsets[i], levelOffsets[i] + levelNumNodes[i])); + return levelBounds; + } + uint64_t size() const { return _numNodes * sizeof(NodeItem); } + static uint64_t size(const uint64_t numItems, const uint16_t nodeSize = 16) + { + if (nodeSize < 2) + throw std::invalid_argument("Node size must be at least 2"); + if (numItems == 0) + throw std::invalid_argument( + "Number of items must be greater than 0"); + const uint16_t nodeSizeMin = + std::min(std::max(nodeSize, static_cast(2)), + static_cast(65535)); + // limit so that resulting size in bytes can be represented by uint64_t + if (numItems > static_cast(1) << 56) + throw std::overflow_error("Number of items must be less than 2^56"); + uint64_t n = numItems; + uint64_t numNodes = n; + do { + n = (n + nodeSizeMin - 1) / nodeSizeMin; + numNodes += n; + } while (n != 1); + return numNodes * sizeof(NodeItem); + } + + void streamWrite(const std::function &writeData) + { + writeData(reinterpret_cast(_nodeItems), + static_cast(_numNodes * sizeof(NodeItem))); + } + + NodeItem getExtent() const { return _extent; } +}; + +} // namespace FlatGeobuf + +#endif diff --git a/CMakeLists.txt b/CMakeLists.txt index a1d6a6a..14e7c47 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,6 +29,7 @@ endif() if(CMAKE_BUILD_TYPE STREQUAL "Debug") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O0 -ggdb") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O0 -ggdb") + add_definitions(-DSPDLOG_ACTIVE_LEVEL=SPDLOG_LEVEL_TRACE) elseif(CMAKE_BUILD_TYPE STREQUAL "Release") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3") diff --git a/src/bindings/pybind11_network.cpp b/src/bindings/pybind11_network.cpp index 6e371f0..68b88ac 100644 --- a/src/bindings/pybind11_network.cpp +++ b/src/bindings/pybind11_network.cpp @@ -104,17 +104,20 @@ void bind_network(py::module &m) std::optional, // std::optional>(&Network::query, py::const_), - "position"_a, // - py::kw_only(), // - "radius"_a, // - "k"_a = std::nullopt, // - "z_max_offset"_a = std::nullopt) + "position"_a, // + py::kw_only(), // + "radius"_a, // + "k"_a = std::nullopt, // + "z_max_offset"_a = std::nullopt, // + py::call_guard()) .def("query", py::overload_cast(&Network::query, py::const_), - "bbox"_a) + "bbox"_a, // + py::call_guard()) // - .def("build", &Network::build) + .def("build", &Network::build, "execution_policy"_a = 2, + py::call_guard()) // .def_static("load", &Network::load, "path"_a) .def("dump", &Network::dump, "path"_a, py::kw_only(), diff --git a/src/bindings/pybind11_packedrtree.cpp b/src/bindings/pybind11_packedrtree.cpp index acb8374..80804c8 100644 --- a/src/bindings/pybind11_packedrtree.cpp +++ b/src/bindings/pybind11_packedrtree.cpp @@ -4,7 +4,7 @@ #include #include -#include "packedrtree.h" +#include "packedrtree.hpp" #include "spdlog/spdlog.h" #include "nano_fmm/types.hpp" @@ -57,7 +57,6 @@ void bind_packedrtree(py::module &m) return self.intersects({minX, minY, maxX, maxY}); }, "minX"_a, "minY"_a, "maxX"_a, "maxY"_a) - .def("toVector", &NodeItem::toVector) .def("to_numpy", [](const NodeItem &self) { return Eigen::Vector4d(self.minX, self.minY, // diff --git a/src/bindings/utils.cpp b/src/bindings/utils.cpp index 036ab02..4ba23a0 100644 --- a/src/bindings/utils.cpp +++ b/src/bindings/utils.cpp @@ -6,6 +6,9 @@ #include "nano_fmm/utils.hpp" +#include "spdlog/spdlog.h" +#include + namespace nano_fmm { namespace py = pybind11; @@ -14,6 +17,38 @@ using rvp = py::return_value_policy; void bind_utils(py::module &m) { + py::add_ostream_redirect(m, "ostream_redirect"); + m // + .def("flush", []() { spdlog::default_logger()->flush(); }) + .def("logging", + [](const std::string &msg) { + spdlog::trace("trace msg: {}", msg); + spdlog::debug("debug msg: {}", msg); + spdlog::info("info msg: {}", msg); + spdlog::warn("warn msg: {}", msg); + spdlog::error("error msg: {}", msg); + spdlog::critical("critical msg: {}", msg); + std::cout << "std::cout: " << msg << std::endl; + std::cerr << "std::cerr: " << msg << std::endl; + }) + .def("set_logging_level", + [](int level) { + spdlog::set_level( + static_cast(level)); + }) + .def("get_logging_level", + []() { return static_cast(spdlog::get_level()); }) + .def("setup", + []() { + auto console_sink = + std::make_shared(); + auto logger = + std::make_shared("logger", console_sink); + spdlog::set_default_logger(logger); + }) + // + ; + m // .def("cheap_ruler_k", &utils::cheap_ruler_k, "latitude"_a) .def("cheap_ruler_k_lookup_table", &utils::cheap_ruler_k_lookup_table, diff --git a/src/nano_fmm/network.cpp b/src/nano_fmm/network.cpp index 889c341..e23faeb 100644 --- a/src/nano_fmm/network.cpp +++ b/src/nano_fmm/network.cpp @@ -3,6 +3,8 @@ #include "nano_fmm/heap.hpp" #include "spdlog/spdlog.h" +#include + namespace nano_fmm { bool Network::add_road(const Eigen::Ref &geom, int64_t road_id) @@ -200,14 +202,23 @@ MatchResult Network::match(const RowVectors &trajectory) const return ret; } -void Network::build() const +void Network::build(int execution_polylicy) const { - for (auto &pair : roads_) { - pair.second.build(); + if (execution_polylicy == 1) { + std::for_each(std::execution::par, roads_.begin(), roads_.end(), + [](auto &pair) { pair.second.build(); }); + } else if (execution_polylicy == 2) { + std::for_each(std::execution::par_unseq, roads_.begin(), roads_.end(), + [](auto &pair) { pair.second.build(); }); + } else { + std::for_each(std::execution::seq, roads_.begin(), roads_.end(), + [](auto &pair) { pair.second.build(); }); } rtree(); } +void Network::reset() const { rtree_.reset(); } + std::unique_ptr Network::load(const std::string &path) { // diff --git a/src/nano_fmm/network.hpp b/src/nano_fmm/network.hpp index 42c63cc..8f111bc 100644 --- a/src/nano_fmm/network.hpp +++ b/src/nano_fmm/network.hpp @@ -8,7 +8,7 @@ #include "nano_fmm/network/ubodt.hpp" #include "nano_fmm/network/match_result.hpp" -#include "packedrtree.h" +#include "packedrtree.hpp" #include #include @@ -53,8 +53,9 @@ struct Network // traj MatchResult match(const RowVectors &trajectory) const; - // build cache (not necessary) - void build() const; + // build cache (not necessary), 0 -> seq, 1 -> par, 2 -> par_unseq + void build(int execution_polylicy = 2) const; + void reset() const; // graph operations // move forward/backward N meters, return ProjectPoint diff --git a/src/source.cpp b/src/source.cpp index bf624ba..e69de29 100644 --- a/src/source.cpp +++ b/src/source.cpp @@ -1 +0,0 @@ -#include "packedrtree.cpp" diff --git a/tests/test_basic.py b/tests/test_basic.py index 7961ca2..6e9ca87 100644 --- a/tests/test_basic.py +++ b/tests/test_basic.py @@ -1,5 +1,11 @@ +import contextlib +import io +import os +import sys +import tempfile import time from collections import defaultdict +from contextlib import contextmanager from typing import Dict, List import numpy as np @@ -82,11 +88,13 @@ def test_cheap_ruler_k(): tic = time.time() fmm.benchmarks.cheap_ruler_k(N) toc = time.time() - print(toc - tic, "secs") + delta = toc - tic + print(delta, "secs") tic = time.time() fmm.benchmarks.cheap_ruler_k_lookup_table(N) toc = time.time() - print(toc - tic, "secs (with lookup)") + delta2 = toc - tic + print(delta2, "secs (with lookup)", f"speed up x{delta/delta2:.3f}") print() @@ -403,3 +411,63 @@ def test_random_stroke(): rc = fmm.RandomColor() stroke = rc.next_hex() assert stroke != "#38b5e9" + + +@contextmanager +def capture_and_discard_output(): + stdout_fileno = sys.stdout.fileno() + stderr_fileno = sys.stderr.fileno() + saved_stdout_fileno = os.dup(stdout_fileno) + saved_stderr_fileno = os.dup(stderr_fileno) + + with tempfile.NamedTemporaryFile(mode="w+") as tempf: + try: + os.dup2(tempf.fileno(), stdout_fileno) + os.dup2(tempf.fileno(), stderr_fileno) + yield tempf + finally: + os.dup2(saved_stdout_fileno, stdout_fileno) + os.dup2(saved_stderr_fileno, stderr_fileno) + os.close(saved_stdout_fileno) + os.close(saved_stderr_fileno) + + +def test_logging(): + fmm.utils.logging("hello one") + fmm.utils.set_logging_level(4) + fmm.utils.logging("hello three") + with fmm.utils.ostream_redirect(stdout=True, stderr=True): + fmm.utils.logging("hello four") + buffer = io.StringIO() + with contextlib.redirect_stdout(buffer): + print("This will be captured") + output_string = buffer.getvalue() + assert output_string == "This will be captured\n" + + buffer = io.StringIO() + with contextlib.redirect_stdout(buffer), contextlib.redirect_stderr(buffer): + with fmm.utils.ostream_redirect(stdout=True, stderr=True): + fmm.utils.logging("hello five") + output_string = buffer.getvalue() + assert output_string == "std::cout: hello five\nstd::cerr: hello five\n" + + # fmm.utils.setup() + + buffer = io.StringIO() + with contextlib.redirect_stdout(buffer), contextlib.redirect_stderr(buffer): + fmm.utils.flush() + with fmm.utils.ostream_redirect(stdout=True, stderr=True): + fmm.utils.logging("hello six") + fmm.utils.flush() + fmm.utils.flush() + output_string = buffer.getvalue() + # assert output_string == "std::cout: hello five\nstd::cerr: hello five\n" + + with capture_and_discard_output() as output: + print("hello world") + fmm.utils.logging("hello seven") + fmm.utils.flush() + output.seek(0) + output.read() + fmm.utils.logging("hello eight") + print(f"Captured: {output}")