Optimize entity-entity collision (#6587)

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

11 files changed, 611 insertions, 78 deletions

diff --git a/src/content_sao.cpp b/src/content_sao.cpp
index 1dc05cd22..0e0c19fce 100644
--- a/src/content_sao.cpp
+++ b/src/content_sao.cpp
@@ -224,6 +224,7 @@ ObjectProperties* UnitSAO::accessObjectProperties()
 
 void UnitSAO::notifyObjectPropertiesModified()
 {
+	m_env->updateActiveObject(this);
 	m_properties_sent = false;
 }
 
@@ -607,6 +608,7 @@ void LuaEntitySAO::setPos(const v3f &pos)
 	if(isAttached())
 		return;
 	m_base_position = pos;
+	m_env->updateActiveObject(this);
 	sendPosition(false, true);
 }
 
@@ -615,6 +617,7 @@ void LuaEntitySAO::moveTo(v3f pos, bool continuous)
 	if(isAttached())
 		return;
 	m_base_position = pos;
+	m_env->updateActiveObject(this);
 	if(!continuous)
 		sendPosition(true, true);
 }
@@ -1102,6 +1105,7 @@ void PlayerSAO::setBasePosition(const v3f &position)
 
 	// This needs to be ran for attachments too
 	ServerActiveObject::setBasePosition(position);
+	m_env->updateActiveObject(this);
 	m_position_not_sent = true;
 }
 
diff --git a/src/irr_aabb3d.h b/src/irr_aabb3d.h
index 73bb2db7a..1e2784036 100644
--- a/src/irr_aabb3d.h
+++ b/src/irr_aabb3d.h
@@ -24,3 +24,5 @@ with this program; if not, write to the Free Software Foundation, Inc.,
 #include <aabbox3d.h>
 
 typedef core::aabbox3d<f32> aabb3f;
+typedef core::aabbox3d<s16> aabb3s16;
+typedef core::aabbox3d<s32> aabb3s32;
diff --git a/src/irr_v3d.h b/src/irr_v3d.h
index 3e95c7913..49c2e60ce 100644
--- a/src/irr_v3d.h
+++ b/src/irr_v3d.h
@@ -18,13 +18,29 @@ with this program; if not, write to the Free Software Foundation, Inc.,
 */
 
 #pragma once
-
 #include "irrlichttypes.h"
-
 #include <vector3d.h>
+#include <functional>
 
 typedef core::vector3df v3f;
 typedef core::vector3d<double> v3d;
 typedef core::vector3d<s16> v3s16;
 typedef core::vector3d<u16> v3u16;
 typedef core::vector3d<s32> v3s32;
+
+namespace std
+{
+template <> struct hash<v3s16>
+{
+	typedef v3s16 argument_type;
+	typedef std::size_t result_type;
+	result_type operator()(const argument_type &s) const noexcept
+	{
+		// clang-format off
+		return static_cast<size_t>((static_cast<u64>(s.X) << 20) ^
+		                           (static_cast<u64>(s.Y) << 10) ^
+		                           (static_cast<u64>(s.Z)));
+		// clang-format on
+	}
+};
+}
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;
+};
diff --git a/src/serverenvironment.cpp b/src/serverenvironment.cpp
index 74b44c730..06cfa5839 100644
--- a/src/serverenvironment.cpp
+++ b/src/serverenvironment.cpp
@@ -996,14 +996,7 @@ bool ServerEnvironment::swapNode(v3s16 p, const MapNode &n)
 void ServerEnvironment::getObjectsInsideRadius(std::vector<u16> &objects, v3f pos,
 	float radius)
 {
-	for (auto &activeObject : m_active_objects) {
-		ServerActiveObject* obj = activeObject.second;
-		u16 id = activeObject.first;
-		v3f objectpos = obj->getBasePosition();
-		if (objectpos.getDistanceFrom(pos) > radius)
-			continue;
-		objects.push_back(id);
-	}
+	objects = m_active_objects.getObjectsInsideRadius(pos, radius);
 }
 
 void ServerEnvironment::clearObjects(ClearObjectsMode mode)
@@ -1011,9 +1004,9 @@ void ServerEnvironment::clearObjects(ClearObjectsMode mode)
 	infostream << "ServerEnvironment::clearObjects(): "
 		<< "Removing all active objects" << std::endl;
 	std::vector<u16> objects_to_remove;
-	for (auto &it : m_active_objects) {
+	for (auto &it : m_active_objects.getObjects()) {
 		u16 id = it.first;
-		ServerActiveObject* obj = it.second;
+		ServerActiveObject* obj = it.second.object;
 		if (obj->getType() == ACTIVEOBJECT_TYPE_PLAYER)
 			continue;
 
@@ -1040,7 +1033,7 @@ void ServerEnvironment::clearObjects(ClearObjectsMode mode)
 
 	// Remove references from m_active_objects
 	for (u16 i : objects_to_remove) {
-		m_active_objects.erase(i);
+		m_active_objects.removeObject(i);
 	}
 
 	// Get list of loaded blocks
@@ -1338,8 +1331,8 @@ void ServerEnvironment::step(float dtime)
 			send_recommended = true;
 		}
 
-		for (auto &ao_it : m_active_objects) {
-			ServerActiveObject* obj = ao_it.second;
+		for (auto &ao_it : m_active_objects.getObjects()) {
+			ServerActiveObject* obj = ao_it.second.object;
 			if (obj->isGone())
 				continue;
 
@@ -1425,40 +1418,7 @@ void ServerEnvironment::deleteParticleSpawner(u32 id, bool remove_from_object)
 
 ServerActiveObject* ServerEnvironment::getActiveObject(u16 id)
 {
-	ServerActiveObjectMap::const_iterator n = m_active_objects.find(id);
-	return (n != m_active_objects.end() ? n->second : NULL);
-}
-
-/**
- * Verify if id is a free active object id
- * @param id
- * @return true if slot is free
- */
-bool ServerEnvironment::isFreeServerActiveObjectId(u16 id) const
-{
-	if (id == 0)
-		return false;
-
-	return m_active_objects.find(id) == m_active_objects.end();
-}
-
-/**
- * Retrieve the first free ActiveObject ID
- * @return free activeobject ID or 0 if none was found
- */
-u16 ServerEnvironment::getFreeServerActiveObjectId()
-{
-	// 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 (isFreeServerActiveObjectId(last_used_id))
-			return last_used_id;
-
-		if (last_used_id == startid)
-			return 0;
-	}
+	return m_active_objects.getObject(id);
 }
 
 u16 ServerEnvironment::addActiveObject(ServerActiveObject *object)
@@ -1469,6 +1429,12 @@ u16 ServerEnvironment::addActiveObject(ServerActiveObject *object)
 	return id;
 }
 
+void ServerEnvironment::updateActiveObject(ServerActiveObject *object)
+{
+	assert(object);
+	m_active_objects.updateObject(object);
+}
+
 /*
 	Finds out what new objects have been added to
 	inside a radius around a position
@@ -1490,11 +1456,11 @@ void ServerEnvironment::getAddedActiveObjects(PlayerSAO *playersao, s16 radius,
 		- discard objects that are found in current_objects.
 		- add remaining objects to added_objects
 	*/
-	for (auto &ao_it : m_active_objects) {
+	for (auto &ao_it : m_active_objects.getObjects()) {
 		u16 id = ao_it.first;
 
 		// Get object
-		ServerActiveObject *object = ao_it.second;
+		ServerActiveObject *object = ao_it.second.object;
 		if (object == NULL)
 			continue;
 
@@ -1578,16 +1544,14 @@ void ServerEnvironment::setStaticForActiveObjectsInBlock(
 
 	for (auto &so_it : block->m_static_objects.m_active) {
 		// Get the ServerActiveObject counterpart to this StaticObject
-		ServerActiveObjectMap::const_iterator ao_it = m_active_objects.find(so_it.first);
-		if (ao_it == m_active_objects.end()) {
+		ServerActiveObject *sao = m_active_objects.getObject(so_it.first);
+		if (!sao) {
 			// If this ever happens, there must be some kind of nasty bug.
 			errorstream << "ServerEnvironment::setStaticForObjectsInBlock(): "
 				"Object from MapBlock::m_static_objects::m_active not found "
 				"in m_active_objects";
 			continue;
 		}
-
-		ServerActiveObject *sao = ao_it->second;
 		sao->m_static_exists = static_exists;
 		sao->m_static_block  = static_block;
 	}
@@ -1644,7 +1608,7 @@ u16 ServerEnvironment::addActiveObjectRaw(ServerActiveObject *object,
 {
 	assert(object); // Pre-condition
 	if(object->getId() == 0){
-		u16 new_id = getFreeServerActiveObjectId();
+		u16 new_id = m_active_objects.getFreeId();
 		if(new_id == 0)
 		{
 			errorstream<<"ServerEnvironment::addActiveObjectRaw(): "
@@ -1660,7 +1624,7 @@ u16 ServerEnvironment::addActiveObjectRaw(ServerActiveObject *object,
 			<<"supplied with id "<<object->getId()<<std::endl;
 	}
 
-	if(!isFreeServerActiveObjectId(object->getId())) {
+	if (!m_active_objects.isFreeId(object->getId())) {
 		errorstream<<"ServerEnvironment::addActiveObjectRaw(): "
 			<<"id is not free ("<<object->getId()<<")"<<std::endl;
 		if(object->environmentDeletes())
@@ -1681,7 +1645,7 @@ u16 ServerEnvironment::addActiveObjectRaw(ServerActiveObject *object,
 	/*infostream<<"ServerEnvironment::addActiveObjectRaw(): "
 			<<"added (id="<<object->getId()<<")"<<std::endl;*/
 
-	m_active_objects[object->getId()] = object;
+	m_active_objects.addObject(object);
 
 	verbosestream<<"ServerEnvironment::addActiveObjectRaw(): "
 		<<"Added id="<<object->getId()<<"; there are now "
@@ -1727,9 +1691,9 @@ u16 ServerEnvironment::addActiveObjectRaw(ServerActiveObject *object,
 void ServerEnvironment::removeRemovedObjects()
 {
 	std::vector<u16> objects_to_remove;
-	for (auto &ao_it : m_active_objects) {
+	for (auto &ao_it : m_active_objects.getObjects()) {
 		u16 id = ao_it.first;
-		ServerActiveObject* obj = ao_it.second;
+		ServerActiveObject* obj = ao_it.second.object;
 
 		// This shouldn't happen but check it
 		if (!obj) {
@@ -1794,7 +1758,7 @@ void ServerEnvironment::removeRemovedObjects()
 	}
 	// Remove references from m_active_objects
 	for (u16 i : objects_to_remove) {
-		m_active_objects.erase(i);
+		m_active_objects.removeObject(i);
 	}
 }
 
@@ -1916,11 +1880,11 @@ void ServerEnvironment::activateObjects(MapBlock *block, u32 dtime_s)
 void ServerEnvironment::deactivateFarObjects(bool _force_delete)
 {
 	std::vector<u16> objects_to_remove;
-	for (auto &ao_it : m_active_objects) {
+	for (auto &ao_it : m_active_objects.getObjects()) {
 		// force_delete might be overriden per object
 		bool force_delete = _force_delete;
 
-		ServerActiveObject* obj = ao_it.second;
+		ServerActiveObject* obj = ao_it.second.object;
 		assert(obj);
 
 		// Do not deactivate if static data creation not allowed
@@ -2051,7 +2015,7 @@ void ServerEnvironment::deactivateFarObjects(bool _force_delete)
 
 	// Remove references from m_active_objects
 	for (u16 i : objects_to_remove) {
-		m_active_objects.erase(i);
+		m_active_objects.removeObject(i);
 	}
 }
 
diff --git a/src/serverenvironment.h b/src/serverenvironment.h
index 225f788d9..05a68cb30 100644
--- a/src/serverenvironment.h
+++ b/src/serverenvironment.h
@@ -22,6 +22,7 @@ with this program; if not, write to the Free Software Foundation, Inc.,
 #include "activeobject.h"
 #include "environment.h"
 #include "mapnode.h"
+#include "server/serveractiveobjectmap.h"
 #include "settings.h"
 #include "util/numeric.h"
 #include <set>
@@ -193,8 +194,6 @@ enum ClearObjectsMode {
 	This is not thread-safe. Server uses an environment mutex.
 */
 
-typedef std::unordered_map<u16, ServerActiveObject *> ServerActiveObjectMap;
-
 class ServerEnvironment : public Environment
 {
 public:
@@ -254,18 +253,7 @@ public:
 	*/
 	u16 addActiveObject(ServerActiveObject *object);
 
-	/**
-	 * Verify if id is a free active object id
-	 * @param id
-	 * @return true if slot is free
-	 */
-	bool isFreeServerActiveObjectId(u16 id) const;
-
-	/**
-	 * Retrieve the first free ActiveObject ID
-	 * @return free activeobject ID or 0 if none was found
-	 */
-	u16 getFreeServerActiveObjectId();
+	void updateActiveObject(ServerActiveObject *object);
 
 	/*
 		Add an active object as a static object to the corresponding
diff --git a/src/unittest/CMakeLists.txt b/src/unittest/CMakeLists.txt
index 733eabae3..768959e5e 100644
--- a/src/unittest/CMakeLists.txt
+++ b/src/unittest/CMakeLists.txt
@@ -22,6 +22,7 @@ set (UNITTEST_SRCS
 	${CMAKE_CURRENT_SOURCE_DIR}/test_serialization.cpp
 	${CMAKE_CURRENT_SOURCE_DIR}/test_settings.cpp
 	${CMAKE_CURRENT_SOURCE_DIR}/test_socket.cpp
+	${CMAKE_CURRENT_SOURCE_DIR}/test_serveractiveobjectmap.cpp
 	${CMAKE_CURRENT_SOURCE_DIR}/test_servermodmanager.cpp
 	${CMAKE_CURRENT_SOURCE_DIR}/test_threading.cpp
 	${CMAKE_CURRENT_SOURCE_DIR}/test_utilities.cpp
diff --git a/src/unittest/test.h b/src/unittest/test.h
index 1102f6d33..a6cd03ed2 100644
--- a/src/unittest/test.h
+++ b/src/unittest/test.h
@@ -99,6 +99,10 @@ class TestFailedException : public std::exception {
 	UASSERT(exception_thrown);            \
 }
 
+#define CONCAT_IMPL(x,y) x##y
+#define CONCAT(x,y) CONCAT_IMPL(x, y)
+#define NEWNAME(prefix) CONCAT(prefix, __COUNTER__)
+
 class IGameDef;
 
 class TestBase {
diff --git a/src/unittest/test_serveractiveobjectmap.cpp b/src/unittest/test_serveractiveobjectmap.cpp
new file mode 100644
index 000000000..42c879229
--- /dev/null
+++ b/src/unittest/test_serveractiveobjectmap.cpp
@@ -0,0 +1,214 @@
+/*
+Minetest
+Copyright (C) 2018 nerzhul, Loic BLOT <loic.blot@unix-experience.fr>
+
+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 "test.h"
+
+#include "server/serveractiveobjectmap.h"
+#include "content_sao.h"
+
+class TestServerActiveObjectMap : public TestBase
+{
+public:
+	TestServerActiveObjectMap() { TestManager::registerTestModule(this); }
+	const char *getName() { return "TestServerActiveObjectMap"; }
+
+	void runTests(IGameDef *gamedef);
+
+	void testAddObject();
+	void testRemoveObject();
+	void testUpdateObject();
+	void testGetObject();
+	void testIsFreeID();
+	void testGetFreeID();
+	void testGetObjectsInsideRadius();
+	void testGetObjectsTouchingBox();
+};
+
+static TestServerActiveObjectMap g_test_instance;
+
+void TestServerActiveObjectMap::runTests(IGameDef *gamedef)
+{
+	TEST(testAddObject);
+	TEST(testRemoveObject);
+	TEST(testUpdateObject);
+	TEST(testGetObject);
+	TEST(testIsFreeID);
+	TEST(testGetFreeID);
+	TEST(testGetObjectsInsideRadius);
+	TEST(testGetObjectsTouchingBox);
+}
+
+void TestServerActiveObjectMap::testAddObject()
+{
+	ServerActiveObjectMap saom;
+	UASSERT(saom.getObjects().empty());
+
+	LuaEntitySAO ob1(nullptr, v3f(0, 0, 0), "", "");
+	ob1.setId(saom.getFreeId());
+	saom.addObject(&ob1);
+
+	UASSERT(saom.getObjects().size() == 1);
+	bool found = false;
+	for (const auto &pair : saom.getObjects()) {
+		UASSERT(pair.second.object == &ob1);
+		found = true;
+	}
+	UASSERT(found);
+}
+
+void TestServerActiveObjectMap::testRemoveObject()
+{
+	ServerActiveObjectMap saom;
+	UASSERT(saom.getObjects().empty());
+
+	LuaEntitySAO ob1(nullptr, v3f(0, 0, 0), "", "");
+	ob1.setId(saom.getFreeId());
+	saom.addObject(&ob1);
+
+	UASSERT(saom.getObjects().size() == 1);
+	bool found = false;
+	for (const auto &pair : saom.getObjects()) {
+		UASSERT(pair.second.object == &ob1);
+		found = true;
+	}
+	UASSERT(found);
+
+	saom.removeObject(&ob1);
+}
+
+void TestServerActiveObjectMap::testUpdateObject()
+{
+	ServerActiveObjectMap saom;
+	LuaEntitySAO ob1(nullptr, v3f(1, 0, 0), "", "");
+	ob1.accessObjectProperties()->physical = true;
+	ob1.setId(saom.getFreeId());
+	saom.addObject(&ob1);
+	UASSERT(saom.getObjectsInsideRadius(v3f(0, 0, 0), 2).size() == 1);
+	UASSERT(saom.getObjectsInsideRadius(v3f(6, 0, 0), 2).size() == 0);
+	ob1.setBasePosition(v3f(5, 0, 0));
+	saom.updateObject(&ob1);
+	UASSERT(saom.getObjectsInsideRadius(v3f(0, 0, 0), 2).size() == 0);
+	UASSERT(saom.getObjectsInsideRadius(v3f(6, 0, 0), 2).size() == 1);
+}
+
+void TestServerActiveObjectMap::testGetObject()
+{
+	ServerActiveObjectMap saom;
+	LuaEntitySAO ob1(nullptr, v3f(0, 0, 0), "", "");
+	u16 id = saom.getFreeId();
+	ob1.setId(id);
+	saom.addObject(&ob1);
+	UASSERT(saom.getObject(0) == nullptr);
+	UASSERT(saom.getObject(id) == &ob1);
+	UASSERT(saom.getObject(id + 1) == nullptr);
+}
+
+void TestServerActiveObjectMap::testIsFreeID()
+{
+	ServerActiveObjectMap saom;
+	UASSERT(!saom.isFreeId(0));
+	UASSERT(saom.isFreeId(1));
+	UASSERT(saom.isFreeId(2));
+}
+
+void TestServerActiveObjectMap::testGetFreeID()
+{
+	ServerActiveObjectMap saom;
+	u16 first_id = saom.getFreeId();
+	UASSERT(first_id > 0);
+	UASSERT(saom.getFreeId() > first_id);
+}
+
+void TestServerActiveObjectMap::testGetObjectsInsideRadius()
+{
+	ServerActiveObjectMap saom;
+#define ADD_OBJECT_IMPL(name, pos)                                                       \
+	LuaEntitySAO name(nullptr, pos, "", "");                                         \
+	name.accessObjectProperties()->physical = true;                                  \
+	name.setId(saom.getFreeId());                                                    \
+	saom.addObject(&name)
+#define ADD_OBJECT(pos) ADD_OBJECT_IMPL(NEWNAME(ob), pos)
+#define OBJECT_COUNT (saom.getObjectsInsideRadius(v3f(0, 0, 0), 5).size())
+
+	UASSERT(OBJECT_COUNT == 0);
+
+	ADD_OBJECT(v3f(0, 0, 0));
+	UASSERT(OBJECT_COUNT == 1);
+
+	ADD_OBJECT(v3f(-1, -1, -1));
+	UASSERT(OBJECT_COUNT == 2);
+
+	ADD_OBJECT(v3f(4.9, 0, 0));
+	UASSERT(OBJECT_COUNT == 3);
+
+	ADD_OBJECT(v3f(5.1, 0, 0));
+	UASSERT(OBJECT_COUNT == 3);
+
+	ADD_OBJECT(v3f(3, 3, 3));
+	UASSERT(OBJECT_COUNT == 3);
+}
+
+void TestServerActiveObjectMap::testGetObjectsTouchingBox()
+{
+	ServerActiveObjectMap saom;
+
+	LuaEntitySAO ob1(nullptr, v3f(1 * BS, 0, 0), "", "");
+	ob1.accessObjectProperties()->physical = true;
+	// Collision boxes are in nodes, not in world units:
+	ob1.accessObjectProperties()->collisionbox = {-1, -1, -1, 1, 1, 1};
+	ob1.setId(saom.getFreeId());
+	saom.addObject(&ob1);
+
+	LuaEntitySAO ob2(nullptr, v3f(1.5 * BS, 2.5 * BS, 0), "", "");
+	ob2.accessObjectProperties()->physical = true;
+	ob2.accessObjectProperties()->collisionbox = {-0.5, -0.5, -1, 3.5, 2, 1};
+	ob2.setId(saom.getFreeId());
+	saom.addObject(&ob2);
+
+	std::vector<u16> list;
+
+	list = saom.getObjectsTouchingBox(
+			{2.1 * BS, -1.0 * BS, -1.0 * BS, 2.5 * BS, 1.0 * BS, 1.0 * BS});
+	UASSERT(list.size() == 0);
+
+	// intersecting ob1
+	list = saom.getObjectsTouchingBox(
+			{1.9 * BS, -1.0 * BS, -1.0 * BS, 2.5 * BS, 1.0 * BS, 1.0 * BS});
+	UASSERT(list.size() == 1 && list[0] == ob1.getId());
+
+	// intersecting ob2
+	list = saom.getObjectsTouchingBox(
+			{2.1 * BS, -1.0 * BS, -1.0 * BS, 2.5 * BS, 2.1 * BS, 1.0 * BS});
+	UASSERT(list.size() == 1 && list[0] == ob2.getId());
+
+	// contained in ob1
+	list = saom.getObjectsTouchingBox(
+			{1.5 * BS, -0.1 * BS, -0.1 * BS, 1.9 * BS, 0.1 * BS, 0.1 * BS});
+	UASSERT(list.size() == 1 && list[0] == ob1.getId());
+
+	// contains ob2
+	list = saom.getObjectsTouchingBox(
+			{0.9 * BS, 1.5 * BS, -5.0 * BS, 6.0 * BS, 20.0 * BS, 500.0 * BS});
+	UASSERT(list.size() == 1 && list[0] == ob2.getId());
+
+	// intersecting both
+	list = saom.getObjectsTouchingBox(
+			{1.9 * BS, -1.0 * BS, -1.0 * BS, 2.5 * BS, 2.1 * BS, 1.0 * BS});
+	UASSERT(list.size() == 2);
+}