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

@@ -36,6 +36,7 @@
 #include <list>
 #include <map>
 #include <set>
+#include <unordered_map>
 
 #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<const uint32_t*>(&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<lut_2d_coordinates_t, std::vector<Simulable::Ptr>, 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
 	{

modules/simulator/src/World.cpp

@@ -277,13 +277,28 @@ std::set<float> World::getElevationsAt(const mrpt::math::TPoint2Df& worldXY) con
 	// Assumption: getListOfSimulableObjectsMtx() is already adquired by all possible call paths?
 	std::set<float> 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;

modules/simulator/src/World_load_xml.cpp

@@ -207,6 +207,7 @@ void World::parse_tag_block(const XmlParserContext& ctx)
 	// <block> 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)

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<int32_t>(p.x / LUT_GRID_SIZE);
+	c.y = static_cast<int32_t>(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<lut_2d_coordinates_t, LutIndexHash> 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);
+	}
+}