Optimize entity-entity collision (#6587)

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

build/android/jni/Android.mk

@@ -221,6 +221,7 @@ LOCAL_SRC_FILES := \
 		jni/src/rollback_interface.cpp            \
 		jni/src/serialization.cpp                 \
 		jni/src/server/mods.cpp                   \
+		jni/src/server/serveractiveobjectmap.cpp  \
 		jni/src/server.cpp                        \
 		jni/src/serverenvironment.cpp             \
 		jni/src/serverlist.cpp                    \

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;
 }
 

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;

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
+	}
+};
+}

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)

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;
+	}
+}