From cde16f7214695530ea20f591e5ffafcff8c4a86a Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Wed, 16 Oct 2024 20:30:53 +0200 Subject: [PATCH] Great performance improvement for worlds with many (>100) block objects. Terrain elevation query function has been refactored to use a 2D hash-map instead of naively visiting all objects. --- modules/simulator/include/mvsim/World.h | 46 ++++++++++++++++++++++++ modules/simulator/src/World.cpp | 21 +++++++++-- modules/simulator/src/World_load_xml.cpp | 1 + modules/simulator/src/World_simul.cpp | 35 ++++++++++++++++++ 4 files changed, 100 insertions(+), 3 deletions(-) diff --git a/modules/simulator/include/mvsim/World.h b/modules/simulator/include/mvsim/World.h index a84886ea..3215a234 100644 --- a/modules/simulator/include/mvsim/World.h +++ b/modules/simulator/include/mvsim/World.h @@ -36,6 +36,7 @@ #include #include #include +#include #if MVSIM_HAS_ZMQ && MVSIM_HAS_PROTOBUF // forward declarations: @@ -594,6 +595,51 @@ class World : public mrpt::system::COutputLogger std::mutex simulationStepRunningMtx_; + // A 2D-hash table of objects + struct lut_2d_coordinates_t + { + int32_t x, y; + + bool operator==(const lut_2d_coordinates_t& o) const noexcept + { + return (x == o.x && y == o.y); + } + }; + + static lut_2d_coordinates_t xy_to_lut_coords(const mrpt::math::TPoint2Df& p); + + struct LutIndexHash + { + std::size_t operator()(const lut_2d_coordinates_t& p) const noexcept + { + // These are the implicit assumptions of the reinterpret cast below: + static_assert(sizeof(int32_t) == sizeof(uint32_t)); + static_assert(offsetof(lut_2d_coordinates_t, x) == 0 * sizeof(uint32_t)); + static_assert(offsetof(lut_2d_coordinates_t, y) == 1 * sizeof(uint32_t)); + + const uint32_t* vec = reinterpret_cast(&p); + return ((1 << 20) - 1) & (vec[0] * 73856093 ^ vec[1] * 19349663); + } + /// k1 < k2? for std::map containers + bool operator()( + const lut_2d_coordinates_t& k1, const lut_2d_coordinates_t& k2) const noexcept + { + if (k1.x != k2.x) return k1.x < k2.x; + return k1.y < k2.y; + } + }; + + using LUTCache = + std::unordered_map, LutIndexHash>; + + /// Ensure the cache is built and up-to-date, then return it: + const LUTCache& getLUTCacheOfObjects() const; + + mutable LUTCache lut2d_objects_; + mutable bool lut2d_objects_is_up_to_date_ = false; + + void internal_update_lut_cache() const; + /** GUI stuff */ struct GUI { diff --git a/modules/simulator/src/World.cpp b/modules/simulator/src/World.cpp index 2b721ff9..30bda06f 100644 --- a/modules/simulator/src/World.cpp +++ b/modules/simulator/src/World.cpp @@ -277,13 +277,28 @@ std::set World::getElevationsAt(const mrpt::math::TPoint2Df& worldXY) con // Assumption: getListOfSimulableObjectsMtx() is already adquired by all possible call paths? std::set ret; - for (const auto& [name, obj] : simulableObjects_) + // Optimized search for potential objects that influence this query: + // 1) world elements: assuming they are few, visit them all. + for (const auto& obj : worldElements_) { - if (!obj) continue; - const auto optZ = obj->getElevationAt(worldXY); if (optZ) ret.insert(*optZ); } + + // 2) blocks: by hashed 2D LUT. + const World::LUTCache& lut = getLUTCacheOfObjects(); + const auto lutCoord = xy_to_lut_coords(worldXY); + if (auto it = lut.find(lutCoord); it != lut.end()) + { + for (const auto& obj : it->second) + { + if (!obj) continue; + const auto optZ = obj->getElevationAt(worldXY); + if (optZ) ret.insert(*optZ); + } + } + + // if none: if (ret.empty()) ret.insert(.0f); return ret; diff --git a/modules/simulator/src/World_load_xml.cpp b/modules/simulator/src/World_load_xml.cpp index 5e750463..0b898ed3 100644 --- a/modules/simulator/src/World_load_xml.cpp +++ b/modules/simulator/src/World_load_xml.cpp @@ -207,6 +207,7 @@ void World::parse_tag_block(const XmlParserContext& ctx) // entries: Block::Ptr block = Block::factory(this, ctx.node); insertBlock(block); + lut2d_objects_is_up_to_date_ = false; } void World::parse_tag_block_class(const XmlParserContext& ctx) diff --git a/modules/simulator/src/World_simul.cpp b/modules/simulator/src/World_simul.cpp index b4ee5141..75783e4c 100644 --- a/modules/simulator/src/World_simul.cpp +++ b/modules/simulator/src/World_simul.cpp @@ -578,3 +578,38 @@ void World::internal_simul_pre_step_terrain_elevation() } } // end for object } + +const World::LUTCache& World::getLUTCacheOfObjects() const +{ + if (!lut2d_objects_is_up_to_date_) internal_update_lut_cache(); + + return lut2d_objects_; +} + +World::lut_2d_coordinates_t World::xy_to_lut_coords(const mrpt::math::TPoint2Df& p) +{ + constexpr float LUT_GRID_SIZE = 4.0; + World::lut_2d_coordinates_t c; + c.x = static_cast(p.x / LUT_GRID_SIZE); + c.y = static_cast(p.y / LUT_GRID_SIZE); + return c; +} + +void World::internal_update_lut_cache() const +{ + lut2d_objects_is_up_to_date_ = true; + + lut2d_objects_.clear(); + for (const auto& [name, obj] : blocks_) + { + std::set affected_coords; + const auto p = obj->getCPose3D(); + for (const auto& vertex : obj->blockShape()) + { + const auto pt = p.composePoint({vertex.x, vertex.y, .0}); + const auto c = xy_to_lut_coords(mrpt::math::TPoint2Df(pt.x, pt.y)); + affected_coords.insert(c); + } + for (const auto& c : affected_coords) lut2d_objects_[c].push_back(obj); + } +}