Optimize entity-entity collision (#6587)

* Add IrrLicht type aliases
* Add hash for IrrLicht vector
* Add object map

3 files changed, 340 insertions, 0 deletions

diff --git a/src/server/CMakeLists.txt b/src/server/CMakeLists.txt
index b892e83b3..98b4730b1 100644
--- a/src/server/CMakeLists.txt
+++ b/src/server/CMakeLists.txt
@@ -1,3 +1,4 @@
 set(server_SRCS
 	${CMAKE_CURRENT_SOURCE_DIR}/mods.cpp
+	${CMAKE_CURRENT_SOURCE_DIR}/serveractiveobjectmap.cpp
 	PARENT_SCOPE)
diff --git a/src/server/serveractiveobjectmap.cpp b/src/server/serveractiveobjectmap.cpp
new file mode 100644
index 000000000..e2def776d
--- /dev/null
+++ b/src/server/serveractiveobjectmap.cpp
@@ -0,0 +1,196 @@
+/*
+Minetest
+Copyright (C) 2018 numZero, Lobachevsky Vitaly <numzer0@yandex.com>
+
+This program is free software; you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation; either version 2.1 of the License, or
+(at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License along
+with this program; if not, write to the Free Software Foundation, Inc.,
+51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+*/
+
+#include "serveractiveobjectmap.h"
+#include "constants.h"
+#include "log.h"
+#include "serverobject.h"
+
+static constexpr float granularity = 16.0 * BS;
+
+static aabb3s16 calcBox(const aabb3f &cb)
+{
+	return aabb3s16(
+			floor(cb.MinEdge.X / granularity),
+			floor(cb.MinEdge.Y / granularity),
+			floor(cb.MinEdge.Z / granularity),
+			ceil(cb.MaxEdge.X / granularity),
+			ceil(cb.MaxEdge.Y / granularity),
+			ceil(cb.MaxEdge.Z / granularity));
+}
+
+void ServerActiveObjectMap::addObject(ServerActiveObject *object)
+{
+	aabb3f cb;
+	Wrapper w;
+	u16 id = object->getId();
+	if (!isFreeId(id))
+		throw std::logic_error("ServerActiveObjectMap::addObject: "
+			"object ID in use: " + std::to_string(id));
+	w.object = object;
+	w.has_box = w.object->getCollisionBox(&cb);
+	if (w.has_box) {
+		w.box = calcBox(cb);
+		addObjectRefs(id, w.box);
+	}
+	objects.emplace(id, w);
+}
+
+ServerActiveObject *ServerActiveObjectMap::removeObject(u16 id)
+{
+	auto pw = objects.find(id);
+	if (pw == objects.end())
+		return nullptr; // silently ignore non-existent object
+	Wrapper w = pw->second;
+	if (w.has_box)
+		removeObjectRefs(id, w.box);
+	objects.erase(pw);
+	return w.object;
+}
+
+void ServerActiveObjectMap::removeObject(ServerActiveObject *object)
+{
+	removeObject(object->getId());
+}
+
+void ServerActiveObjectMap::updateObject(u16 id)
+{
+	auto pw = objects.find(id);
+	if (pw == objects.end()) {
+		warningstream << "Trying to update non-existent object: " << id
+				<< std::endl;
+		return;
+	}
+	Wrapper &w = pw->second;
+	aabb3f cb;
+	aabb3s16 box;
+	bool has_box = w.object->getCollisionBox(&cb);
+	if (has_box)
+		box = calcBox(cb);
+	if (w.has_box && has_box && w.box == box)
+		return;
+	if (w.has_box)
+		removeObjectRefs(id, w.box);
+	w.box = box;
+	w.has_box = has_box;
+	if (w.has_box)
+		addObjectRefs(id, w.box);
+}
+
+void ServerActiveObjectMap::updateObject(ServerActiveObject *object)
+{
+	updateObject(object->getId());
+}
+
+ServerActiveObject *ServerActiveObjectMap::getObject(u16 id) const
+{
+	auto pw = objects.find(id);
+	if (pw == objects.end())
+		return nullptr;
+	return pw->second.object;
+}
+
+std::vector<u16> ServerActiveObjectMap::getObjectsInsideRadius(v3f pos, float radius)
+{
+	std::vector<u16> result;
+	auto nearby = getObjectsNearBox(calcBox({pos - radius, pos + radius}));
+	for (auto &id : nearby) {
+		ServerActiveObject *obj = getObject(id);
+		v3f objectpos = obj->getBasePosition();
+		if (objectpos.getDistanceFrom(pos) > radius)
+			continue;
+		result.push_back(id);
+	}
+	return result;
+}
+
+std::vector<u16> ServerActiveObjectMap::getObjectsTouchingBox(const aabb3f &box)
+{
+	std::vector<u16> result;
+	auto nearby = getObjectsNearBox(calcBox(box));
+	for (auto &id : nearby) {
+		ServerActiveObject *obj = getObject(id);
+		aabb3f cb;
+		if (!obj->getCollisionBox(&cb))
+			continue;
+		if (!box.intersectsWithBox(cb))
+			continue;
+		result.push_back(id);
+	}
+	return result;
+}
+
+std::unordered_set<u16> ServerActiveObjectMap::getObjectsNearBox(const aabb3s16 &box)
+{
+	std::unordered_set<u16> result;
+	v3s16 p;
+	for (p.Z = box.MinEdge.Z; p.Z <= box.MaxEdge.Z; p.Z++)
+	for (p.Y = box.MinEdge.Y; p.Y <= box.MaxEdge.Y; p.Y++)
+	for (p.X = box.MinEdge.X; p.X <= box.MaxEdge.X; p.X++) {
+		auto bounds = refmap.equal_range(p);
+		for (auto iter = bounds.first; iter != bounds.second; ++iter)
+			result.insert(iter->second);
+	}
+	return result;
+}
+
+void ServerActiveObjectMap::addObjectRefs(u16 id, const aabb3s16 &box)
+{
+	v3s16 p;
+	for (p.Z = box.MinEdge.Z; p.Z <= box.MaxEdge.Z; p.Z++)
+	for (p.Y = box.MinEdge.Y; p.Y <= box.MaxEdge.Y; p.Y++)
+	for (p.X = box.MinEdge.X; p.X <= box.MaxEdge.X; p.X++)
+		refmap.emplace(p, id);
+}
+
+void ServerActiveObjectMap::removeObjectRefs(u16 id, const aabb3s16 &box)
+{
+	v3s16 p;
+	for (p.Z = box.MinEdge.Z; p.Z <= box.MaxEdge.Z; p.Z++)
+	for (p.Y = box.MinEdge.Y; p.Y <= box.MaxEdge.Y; p.Y++)
+	for (p.X = box.MinEdge.X; p.X <= box.MaxEdge.X; p.X++) {
+		auto bounds = refmap.equal_range(p);
+		for (auto iter = bounds.first; iter != bounds.second;)
+			if (iter->second == id)
+				refmap.erase(iter++);
+			else
+				++iter;
+	}
+}
+
+bool ServerActiveObjectMap::isFreeId(u16 id)
+{
+	if (id == 0)
+		return false;
+	return objects.find(id) == objects.end();
+}
+
+u16 ServerActiveObjectMap::getFreeId()
+{
+	// try to reuse id's as late as possible
+	static u16 last_used_id = 0;
+	u16 startid = last_used_id;
+	for (;;) {
+		last_used_id++;
+		if (isFreeId(last_used_id))
+			return last_used_id;
+		if (last_used_id == startid) // wrapped around
+			return 0;
+	}
+}
diff --git a/src/server/serveractiveobjectmap.h b/src/server/serveractiveobjectmap.h
new file mode 100644
index 000000000..53ac95e27
--- /dev/null
+++ b/src/server/serveractiveobjectmap.h
@@ -0,0 +1,143 @@
+/*
+Minetest
+Copyright (C) 2018 numZero, Lobachevsky Vitaly <numzer0@yandex.com>
+
+This program is free software; you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation; either version 2.1 of the License, or
+(at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License along
+with this program; if not, write to the Free Software Foundation, Inc.,
+51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+*/
+
+#pragma once
+#include <unordered_set>
+#include <unordered_map>
+#include <vector>
+#include "irr_v3d.h"
+#include "irr_aabb3d.h"
+
+class ServerActiveObject;
+
+/*!
+ * The class to speed up collision tests.
+ *
+ * @note It stores any objects but only those that has valid collision box
+ * (`physical` Lua entities) are actually processed.
+ * @note It uses world coordinate units, i.e. node size is always BS.
+ */
+struct ServerActiveObjectMap
+{
+	struct Wrapper
+	{
+		ServerActiveObject *object;
+		aabb3s16 box;
+		bool has_box;
+	};
+
+	/*!
+	 * Adds object to the map. It must have valid ID.
+	 *
+	 * If an object with the same ID already exists in the map,
+	 * std::logic_error is thrown.
+	 */
+	void addObject(ServerActiveObject *object);
+
+	/*!
+	 * Removes object from the map. The pointer must be valid.
+	 * See `removeObject(u16)` for details.
+	 */
+	void removeObject(ServerActiveObject *object);
+
+	/*!
+	 * Removes object from the map.
+	 *
+	 * If the object is not found, the call is ignored.
+	 * The function never throws, unless the underlying container throws.
+	 */
+	ServerActiveObject *removeObject(u16 id);
+
+	/*!
+	 * Updates object metadata stored in the map.
+	 * See `updateObject(u16)` for details.
+	 */
+	void updateObject(ServerActiveObject *object);
+
+	/*!
+	 * Updates object metadata stored in the map.
+	 *
+	 * The metadata includes (approximate) absolute collision box and
+	 * its existence (`physical` property for Lua entities).
+	 * This function must be called after each change of these properties,
+	 * including each object movement.
+	 */
+	void updateObject(u16 id);
+
+	/*!
+	 * Returns the object with given ID, if any.
+	 * Returns NULL otherwise.
+	 */
+	ServerActiveObject *getObject(u16 id) const;
+
+	/*!
+	 * Checks if the given ID is free and valid (i.e. non-zero).
+	 */
+	bool isFreeId(u16 id);
+
+	/*!
+	 * Returns a free ID, if any. Returns 0 in the case of failure.
+	 *
+	 * @note This function doesn't reserve the ID; it remains free until
+	 * an object with that ID is added.
+	 * @note This function tries to reclaim freed IDs as late as possible.
+	 * However, there is no guarantee.
+	 */
+	u16 getFreeId();
+
+	/*!
+	 * Returns a list of objects whose base position is at distance less
+	 * than @p radius from @p pos.
+	 *
+	 * @note Due to inexact nature of floating-point computations, it is
+	 * undefined whether an object lying exactly at the boundary is included
+	 * in the list or not.
+	 * @note Objects with base position outside of the collision box may not
+	 * be returned.
+	 * @note Objects without valid collision box are not returned.
+	 */
+	std::vector<u16> getObjectsInsideRadius(v3f pos, float radius);
+
+	/*!
+	 * Returns a list of objects whose collision box intersects with @p box
+	 *
+	 * @note Due to inexact nature of floating-point computations, it is
+	 * undefined whether an object lying exactly at the boundary is included
+	 * in the list or not.
+	 */
+	std::vector<u16> getObjectsTouchingBox(const aabb3f &box);
+
+	/*!
+	 * Returns count of objects in the map.
+	 */
+	std::size_t size() const { return objects.size(); }
+
+	/*!
+	 * Returns reference to the underlying container.
+	 */
+	const std::unordered_map<u16, Wrapper> &getObjects() const { return objects; }
+
+private:
+	void addObjectRefs(u16 id, const aabb3s16 &box);
+	void removeObjectRefs(u16 id, const aabb3s16 &box);
+	std::unordered_set<u16> getObjectsNearBox(const aabb3s16 &box);
+
+	std::unordered_map<u16, Wrapper> objects;
+	std::unordered_multimap<v3s16, u16> refmap;
+};