Added engine containers.

core/containers/bin_sorted_array.h

@@ -0,0 +1,181 @@
+/**************************************************************************/
+/*  bin_sorted_array.h                                                    */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef BIN_SORTED_ARRAY_H
+#define BIN_SORTED_ARRAY_H
+
+#include "core/containers/local_vector.h"
+#include "core/containers/paged_array.h"
+
+template <class T>
+class BinSortedArray {
+	PagedArray<T> array;
+	LocalVector<uint64_t> bin_limits;
+
+	// Implement if elements need to keep track of their own index in the array.
+	_FORCE_INLINE_ virtual void _update_idx(T &r_element, uint64_t p_idx) {}
+
+	_FORCE_INLINE_ void _swap(uint64_t p_a, uint64_t p_b) {
+		SWAP(array[p_a], array[p_b]);
+		_update_idx(array[p_a], p_a);
+		_update_idx(array[p_b], p_b);
+	}
+
+public:
+	uint64_t insert(T &p_element, uint64_t p_bin) {
+		array.push_back(p_element);
+		uint64_t new_idx = array.size() - 1;
+		_update_idx(p_element, new_idx);
+		bin_limits[0] = new_idx;
+		if (p_bin != 0) {
+			new_idx = move(new_idx, p_bin);
+		}
+		return new_idx;
+	}
+
+	uint64_t move(uint64_t p_idx, uint64_t p_bin) {
+		ERR_FAIL_UNSIGNED_INDEX_V(p_idx, array.size(), -1);
+
+		uint64_t current_bin = bin_limits.size() - 1;
+		while (p_idx > bin_limits[current_bin]) {
+			current_bin--;
+		}
+
+		if (p_bin == current_bin) {
+			return p_idx;
+		}
+
+		uint64_t current_idx = p_idx;
+		if (p_bin > current_bin) {
+			while (p_bin > current_bin) {
+				uint64_t swap_idx = 0;
+
+				if (current_bin == bin_limits.size() - 1) {
+					bin_limits.push_back(0);
+				} else {
+					bin_limits[current_bin + 1]++;
+					swap_idx = bin_limits[current_bin + 1];
+				}
+
+				if (current_idx != swap_idx) {
+					_swap(current_idx, swap_idx);
+					current_idx = swap_idx;
+				}
+
+				current_bin++;
+			}
+		} else {
+			while (p_bin < current_bin) {
+				uint64_t swap_idx = bin_limits[current_bin];
+
+				if (current_idx != swap_idx) {
+					_swap(current_idx, swap_idx);
+				}
+
+				if (current_bin == bin_limits.size() - 1 && bin_limits[current_bin] == 0) {
+					bin_limits.resize(bin_limits.size() - 1);
+				} else {
+					bin_limits[current_bin]--;
+				}
+				current_idx = swap_idx;
+				current_bin--;
+			}
+		}
+
+		return current_idx;
+	}
+
+	void remove_at(uint64_t p_idx) {
+		ERR_FAIL_UNSIGNED_INDEX(p_idx, array.size());
+		uint64_t new_idx = move(p_idx, 0);
+		uint64_t swap_idx = array.size() - 1;
+
+		if (new_idx != swap_idx) {
+			_swap(new_idx, swap_idx);
+		}
+
+		if (bin_limits[0] > 0) {
+			bin_limits[0]--;
+		}
+
+		array.pop_back();
+	}
+
+	void set_page_pool(PagedArrayPool<T> *p_page_pool) {
+		array.set_page_pool(p_page_pool);
+	}
+
+	_FORCE_INLINE_ const T &operator[](uint64_t p_index) const {
+		return array[p_index];
+	}
+
+	_FORCE_INLINE_ T &operator[](uint64_t p_index) {
+		return array[p_index];
+	}
+
+	int get_bin_count() {
+		if (array.size() == 0) {
+			return 0;
+		}
+		return bin_limits.size();
+	}
+
+	int get_bin_start(int p_bin) {
+		ERR_FAIL_COND_V(p_bin >= get_bin_count(), ~0U);
+		if ((unsigned int)p_bin == bin_limits.size() - 1) {
+			return 0;
+		}
+		return bin_limits[p_bin + 1] + 1;
+	}
+
+	int get_bin_size(int p_bin) {
+		ERR_FAIL_COND_V(p_bin >= get_bin_count(), 0);
+		if ((unsigned int)p_bin == bin_limits.size() - 1) {
+			return bin_limits[p_bin] + 1;
+		}
+		return bin_limits[p_bin] - bin_limits[p_bin + 1];
+	}
+
+	void reset() {
+		array.reset();
+		bin_limits.clear();
+		bin_limits.push_back(0);
+	}
+
+	BinSortedArray() {
+		bin_limits.push_back(0);
+	}
+
+	virtual ~BinSortedArray() {
+		reset();
+	}
+};
+
+#endif // BIN_SORTED_ARRAY_H

core/containers/cowdata.h

@@ -0,0 +1,392 @@
+#ifndef COWDATA_H_
+#define COWDATA_H_
+/*************************************************************************/
+/*  cowdata.h                                                            */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include <string.h>
+
+#include "core/error/error_macros.h"
+#include "core/os/memory.h"
+#include "core/os/safe_refcount.h"
+
+template <class T>
+class Vector;
+class String;
+class Char16String;
+class CharString;
+template <class T, class V>
+class VMap;
+
+#if !defined(NO_THREADS)
+SAFE_NUMERIC_TYPE_PUN_GUARANTEES(uint32_t)
+#endif
+
+template <class T>
+class CowData {
+	template <class TV>
+	friend class Vector;
+	friend class String;
+	friend class Char16String;
+	friend class CharString;
+	template <class TV, class VV>
+	friend class VMap;
+
+private:
+	mutable T *_ptr;
+
+	// internal helpers
+
+	_FORCE_INLINE_ SafeNumeric<uint32_t> *_get_refcount() const {
+		if (!_ptr) {
+			return nullptr;
+		}
+
+		return reinterpret_cast<SafeNumeric<uint32_t> *>(_ptr) - 2;
+	}
+
+	_FORCE_INLINE_ uint32_t *_get_size() const {
+		if (!_ptr) {
+			return nullptr;
+		}
+
+		return reinterpret_cast<uint32_t *>(_ptr) - 1;
+	}
+
+	_FORCE_INLINE_ size_t _get_alloc_size(size_t p_elements) const {
+		//return nearest_power_of_2_templated(p_elements*sizeof(T)+sizeof(SafeRefCount)+sizeof(int));
+		return next_power_of_2(p_elements * sizeof(T));
+	}
+
+	_FORCE_INLINE_ bool _get_alloc_size_checked(size_t p_elements, size_t *out) const {
+#if defined(_add_overflow) && defined(_mul_overflow)
+		size_t o;
+		size_t p;
+		if (_mul_overflow(p_elements, sizeof(T), &o)) {
+			*out = 0;
+			return false;
+		}
+		*out = next_power_of_2(o);
+		if (_add_overflow(o, static_cast<size_t>(32), &p)) {
+			return false; //no longer allocated here
+		}
+		return true;
+#else
+		// Speed is more important than correctness here, do the operations unchecked
+		// and hope the best
+		*out = _get_alloc_size(p_elements);
+		return true;
+#endif
+	}
+
+	void _unref(void *p_data);
+	void _ref(const CowData *p_from);
+	void _ref(const CowData &p_from);
+	uint32_t _copy_on_write();
+
+public:
+	void operator=(const CowData<T> &p_from) { _ref(p_from); }
+
+	_FORCE_INLINE_ T *ptrw() {
+		_copy_on_write();
+		return _ptr;
+	}
+
+	_FORCE_INLINE_ const T *ptr() const {
+		return _ptr;
+	}
+
+	_FORCE_INLINE_ int size() const {
+		uint32_t *size = (uint32_t *)_get_size();
+		if (size) {
+			return *size;
+		} else {
+			return 0;
+		}
+	}
+
+	_FORCE_INLINE_ void clear() { resize(0); }
+	_FORCE_INLINE_ bool empty() const { return _ptr == nullptr; }
+
+	_FORCE_INLINE_ void set(int p_index, const T &p_elem) {
+		CRASH_BAD_INDEX(p_index, size());
+		_copy_on_write();
+		_ptr[p_index] = p_elem;
+	}
+
+	_FORCE_INLINE_ T &get_m(int p_index) {
+		CRASH_BAD_INDEX(p_index, size());
+		_copy_on_write();
+		return _ptr[p_index];
+	}
+
+	_FORCE_INLINE_ const T &get(int p_index) const {
+		CRASH_BAD_INDEX(p_index, size());
+
+		return _ptr[p_index];
+	}
+
+	Error resize(int p_size);
+
+	_FORCE_INLINE_ void remove(int p_index) {
+		ERR_FAIL_INDEX(p_index, size());
+		T *p = ptrw();
+		int len = size();
+		for (int i = p_index; i < len - 1; i++) {
+			p[i] = p[i + 1];
+		};
+
+		resize(len - 1);
+	}
+
+	Error insert(int p_pos, const T &p_val) {
+		ERR_FAIL_INDEX_V(p_pos, size() + 1, ERR_INVALID_PARAMETER);
+		resize(size() + 1);
+		for (int i = (size() - 1); i > p_pos; i--) {
+			set(i, get(i - 1));
+		}
+		set(p_pos, p_val);
+
+		return OK;
+	}
+
+	void fill(const T &p_val) {
+		int len = size();
+
+		if (len == 0) {
+			return;
+		}
+
+		T *p = ptrw();
+		for (int i = 0; i < len; ++i) {
+			p[i] = p_val;
+		}
+	}
+
+	int find(const T &p_val, int p_from = 0) const;
+
+	_FORCE_INLINE_ CowData();
+	_FORCE_INLINE_ ~CowData();
+	_FORCE_INLINE_ CowData(CowData<T> &p_from) { _ref(p_from); };
+};
+
+template <class T>
+void CowData<T>::_unref(void *p_data) {
+	if (!p_data) {
+		return;
+	}
+
+	SafeNumeric<uint32_t> *refc = _get_refcount();
+
+	if (refc->decrement() > 0) {
+		return; // still in use
+	}
+	// clean up
+
+	if (!HAS_TRIVIAL_DESTRUCTOR(T)) {
+		uint32_t *count = _get_size();
+		T *data = (T *)(count + 1);
+
+		for (uint32_t i = 0; i < *count; ++i) {
+			// call destructors
+			data[i].~T();
+		}
+	}
+
+	// free mem
+	Memory::free_static((uint8_t *)p_data, true);
+}
+
+template <class T>
+uint32_t CowData<T>::_copy_on_write() {
+	if (!_ptr) {
+		return 0;
+	}
+
+	SafeNumeric<uint32_t> *refc = _get_refcount();
+
+	uint32_t rc = refc->get();
+	if (likely(rc > 1)) {
+		/* in use by more than me */
+		uint32_t current_size = *_get_size();
+
+		uint32_t *mem_new = (uint32_t *)Memory::alloc_static(_get_alloc_size(current_size), true);
+
+		new (mem_new - 2, sizeof(uint32_t), "") SafeNumeric<uint32_t>(1); //refcount
+		*(mem_new - 1) = current_size; //size
+
+		T *_data = (T *)(mem_new);
+
+		// initialize new elements
+		if (HAS_TRIVIAL_COPY(T)) {
+			memcpy(mem_new, _ptr, current_size * sizeof(T));
+
+		} else {
+			for (uint32_t i = 0; i < current_size; i++) {
+				memnew_placement(&_data[i], T(_ptr[i]));
+			}
+		}
+
+		_unref(_ptr);
+		_ptr = _data;
+
+		rc = 1;
+	}
+	return rc;
+}
+
+template <class T>
+Error CowData<T>::resize(int p_size) {
+	ERR_FAIL_COND_V(p_size < 0, ERR_INVALID_PARAMETER);
+
+	int current_size = size();
+
+	if (p_size == current_size) {
+		return OK;
+	}
+
+	if (p_size == 0) {
+		// wants to clean up
+		_unref(_ptr);
+		_ptr = nullptr;
+		return OK;
+	}
+
+	// possibly changing size, copy on write
+	uint32_t rc = _copy_on_write();
+
+	size_t current_alloc_size = _get_alloc_size(current_size);
+	size_t alloc_size;
+	ERR_FAIL_COND_V(!_get_alloc_size_checked(p_size, &alloc_size), ERR_OUT_OF_MEMORY);
+
+	if (p_size > current_size) {
+		if (alloc_size != current_alloc_size) {
+			if (current_size == 0) {
+				// alloc from scratch
+				uint32_t *ptr = (uint32_t *)Memory::alloc_static(alloc_size, true);
+				ERR_FAIL_COND_V(!ptr, ERR_OUT_OF_MEMORY);
+				*(ptr - 1) = 0; //size, currently none
+				new (ptr - 2, sizeof(uint32_t), "") SafeNumeric<uint32_t>(1); //refcount
+
+				_ptr = (T *)ptr;
+
+			} else {
+				uint32_t *_ptrnew = (uint32_t *)Memory::realloc_static(_ptr, alloc_size, true);
+				ERR_FAIL_COND_V(!_ptrnew, ERR_OUT_OF_MEMORY);
+				new (_ptrnew - 2, sizeof(uint32_t), "") SafeNumeric<uint32_t>(rc); //refcount
+
+				_ptr = (T *)(_ptrnew);
+			}
+		}
+
+		// construct the newly created elements
+
+		if (!HAS_TRIVIAL_CONSTRUCTOR(T)) {
+			for (int i = *_get_size(); i < p_size; i++) {
+				memnew_placement(&_ptr[i], T);
+			}
+		}
+
+		*_get_size() = p_size;
+
+	} else if (p_size < current_size) {
+		if (!HAS_TRIVIAL_DESTRUCTOR(T)) {
+			// deinitialize no longer needed elements
+			for (uint32_t i = p_size; i < *_get_size(); i++) {
+				T *t = &_ptr[i];
+				t->~T();
+			}
+		}
+
+		if (alloc_size != current_alloc_size) {
+			uint32_t *_ptrnew = (uint32_t *)Memory::realloc_static(_ptr, alloc_size, true);
+			ERR_FAIL_COND_V(!_ptrnew, ERR_OUT_OF_MEMORY);
+			new (_ptrnew - 2, sizeof(uint32_t), "") SafeNumeric<uint32_t>(rc); //refcount
+
+			_ptr = (T *)(_ptrnew);
+		}
+
+		*_get_size() = p_size;
+	}
+
+	return OK;
+}
+
+template <class T>
+int CowData<T>::find(const T &p_val, int p_from) const {
+	int ret = -1;
+
+	if (p_from < 0 || size() == 0) {
+		return ret;
+	}
+
+	for (int i = p_from; i < size(); i++) {
+		if (get(i) == p_val) {
+			ret = i;
+			break;
+		}
+	}
+
+	return ret;
+}
+
+template <class T>
+void CowData<T>::_ref(const CowData *p_from) {
+	_ref(*p_from);
+}
+
+template <class T>
+void CowData<T>::_ref(const CowData &p_from) {
+	if (_ptr == p_from._ptr) {
+		return; // self assign, do nothing.
+	}
+
+	_unref(_ptr);
+	_ptr = nullptr;
+
+	if (!p_from._ptr) {
+		return; //nothing to do
+	}
+
+	if (p_from._get_refcount()->conditional_increment() > 0) { // could reference
+		_ptr = p_from._ptr;
+	}
+}
+
+template <class T>
+CowData<T>::CowData() {
+	_ptr = nullptr;
+}
+
+template <class T>
+CowData<T>::~CowData() {
+	_unref(_ptr);
+}
+
+#endif /* COW_H_ */

core/containers/fixed_array.h

@@ -0,0 +1,145 @@
+/**************************************************************************/
+/*  fixed_array.h                                                         */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef FIXED_ARRAY_H
+#define FIXED_ARRAY_H
+
+#include "core/typedefs.h"
+
+#include "core/containers/local_vector.h"
+
+// High performance fixed size array, single threaded.
+// Especially useful if you need to create an array on the stack, to
+// prevent dynamic allocations (especially in bottleneck code).
+
+template <class T, uint32_t CAPACITY = 8, bool force_trivial = false, uint32_t ALIGN = 1>
+class FixedArray {
+	static_assert(ALIGN > 0, "ALIGN must be at least 1.");
+	const static uint32_t UNIT_SIZE = ((sizeof(T) + ALIGN - 1) / ALIGN * ALIGN);
+	const static bool CONSTRUCT = !HAS_TRIVIAL_CONSTRUCTOR(T) && !force_trivial;
+	const static bool DESTRUCT = !HAS_TRIVIAL_DESTRUCTOR(T) && !force_trivial;
+
+	uint32_t _size = 0;
+	uint8_t _data[CAPACITY * UNIT_SIZE];
+
+	const T &get(uint32_t p_index) const {
+		return *(const T *)&_data[p_index * UNIT_SIZE];
+	}
+    
+	T &get(uint32_t p_index) {
+		return *(T *)&_data[p_index * UNIT_SIZE];
+	}
+
+public:
+	uint32_t size() const { return _size; }
+	bool is_empty() const { return !_size; }
+	bool is_full() const { return _size == CAPACITY; }
+	uint32_t capacity() const { return CAPACITY; }
+
+	T *request(bool p_construct = true) {
+		if (size() < CAPACITY) {
+			T *ele = &get(_size++);
+			if (CONSTRUCT && p_construct) {
+				memnew_placement(ele, T);
+			}
+			return ele;
+		}
+		return nullptr;
+	}
+
+	void push_back(const T &p_val) {
+		T *mem = request(false);
+		ERR_FAIL_NULL(mem);
+		*mem = p_val;
+	}
+
+	void clear() {
+		resize(0);
+	}
+
+	void remove_unordered(uint32_t p_index) {
+		ERR_FAIL_UNSIGNED_INDEX(p_index, _size);
+
+		_size--;
+		if (_size > p_index) {
+			get(p_index) = get(_size);
+		}
+
+		if (DESTRUCT) {
+			get(_size).~T();
+		}
+	}
+
+	void resize(uint32_t p_size) {
+		ERR_FAIL_COND(p_size > CAPACITY);
+
+		if (DESTRUCT && (p_size < _size)) {
+			for (uint32_t i = p_size; i < _size; i++) {
+				get(i).~T();
+			}
+		}
+
+		if (CONSTRUCT && (p_size > _size)) {
+			for (uint32_t i = _size; i < p_size; i++) {
+				memnew_placement(&get(i), T);
+			}
+		}
+
+		_size = p_size;
+	}
+
+	const T &operator[](uint32_t p_index) const {
+		DEV_ASSERT(p_index < size());
+		return get(p_index);
+	}
+
+	T &operator[](uint32_t p_index) {
+		DEV_ASSERT(p_index < size());
+		return get(p_index);
+	}
+
+	operator Vector<T>() const {
+		Vector<T> ret;
+		if (size()) {
+			ret.resize(size());
+			T *dest = ret.ptrw();
+			if (ALIGN <= 1) {
+				memcpy(dest, _data, sizeof(T) * _size);
+			} else {
+				for (uint32_t n = 0; n < _size; n++) {
+					dest[n] = get(n);
+				}
+			}
+		}
+		return ret;
+	}
+};
+
+#endif // FIXED_ARRAY_H

core/containers/hash_map.h

@@ -0,0 +1,665 @@
+#ifndef HASH_MAP_H
+#define HASH_MAP_H
+
+/**************************************************************************/
+/*  hash_map.h                                                            */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "core/containers/hashfuncs.h"
+#include "core/containers/paged_allocator.h"
+#include "core/containers/pair.h"
+#include "core/math/math_funcs.h"
+#include "core/os/memory.h"
+
+/**
+ * A HashMap implementation that uses open addressing with Robin Hood hashing.
+ * Robin Hood hashing swaps out entries that have a smaller probing distance
+ * than the to-be-inserted entry, that evens out the average probing distance
+ * and enables faster lookups. Backward shift deletion is employed to further
+ * improve the performance and to avoid infinite loops in rare cases.
+ *
+ * Keys and values are stored in a double linked list by insertion order. This
+ * has a slight performance overhead on lookup, which can be mostly compensated
+ * using a paged allocator if required.
+ *
+ * The assignment operator copy the pairs from one map to the other.
+ */
+
+template <class TKey, class TValue, class Hasher = HashMapHasherDefault, class Comparator = HashMapComparatorDefault<TKey>>
+class HashMap {
+public:
+	const uint32_t MIN_CAPACITY_INDEX = 2; // Use a prime.
+	const float MAX_OCCUPANCY = 0.75;
+	const uint32_t EMPTY_HASH = 0;
+
+public:
+	struct Element {
+		Element *next = nullptr;
+		Element *prev = nullptr;
+		KeyValue<TKey, TValue> data;
+
+		const TKey &key() const {
+			return data.key;
+		}
+
+		TValue &value() {
+			return data.value;
+		}
+
+		const TValue &value() const {
+			return data.value;
+		}
+
+		TValue &get() {
+			return data.value;
+		};
+		const TValue &get() const {
+			return data.value;
+		};
+
+		Element() {}
+		Element(const TKey &p_key, const TValue &p_value) :
+				data(p_key, p_value) {}
+	};
+
+public:
+	_FORCE_INLINE_ uint32_t get_capacity() const { return hash_table_size_primes[capacity_index]; }
+	_FORCE_INLINE_ uint32_t size() const { return num_elements; }
+
+	/* Standard Godot Container API */
+
+	bool empty() const {
+		return num_elements == 0;
+	}
+
+	void clear() {
+		if (elements == nullptr || num_elements == 0) {
+			return;
+		}
+		uint32_t capacity = hash_table_size_primes[capacity_index];
+		for (uint32_t i = 0; i < capacity; i++) {
+			if (hashes[i] == EMPTY_HASH) {
+				continue;
+			}
+
+			hashes[i] = EMPTY_HASH;
+			memdelete(elements[i]);
+			elements[i] = nullptr;
+		}
+
+		tail_element = nullptr;
+		head_element = nullptr;
+		num_elements = 0;
+	}
+
+	TValue &get(const TKey &p_key) {
+		uint32_t pos = 0;
+		bool exists = _lookup_pos(p_key, pos);
+		CRASH_COND_MSG(!exists, "HashMap key not found.");
+		return elements[pos]->data.value;
+	}
+
+	const TValue &get(const TKey &p_key) const {
+		uint32_t pos = 0;
+		bool exists = _lookup_pos(p_key, pos);
+		CRASH_COND_MSG(!exists, "HashMap key not found.");
+		return elements[pos]->data.value;
+	}
+
+	const TValue *getptr(const TKey &p_key) const {
+		uint32_t pos = 0;
+		bool exists = _lookup_pos(p_key, pos);
+
+		if (exists) {
+			return &elements[pos]->data.value;
+		}
+		return nullptr;
+	}
+
+	TValue *getptr(const TKey &p_key) {
+		uint32_t pos = 0;
+		bool exists = _lookup_pos(p_key, pos);
+
+		if (exists) {
+			return &elements[pos]->data.value;
+		}
+		return nullptr;
+	}
+
+	const Element *get_element(const TKey &p_key) const {
+		uint32_t pos = 0;
+		bool exists = _lookup_pos(p_key, pos);
+
+		if (exists) {
+			return elements[pos];
+		}
+
+		return NULL;
+	}
+
+	Element *get_element(const TKey &p_key) {
+		uint32_t pos = 0;
+		bool exists = _lookup_pos(p_key, pos);
+
+		if (exists) {
+			return elements[pos];
+		}
+
+		return NULL;
+	}
+
+	_FORCE_INLINE_ const Element *find(const TKey &p_key) const {
+		return get_element(p_key);
+	}
+
+	_FORCE_INLINE_ Element *find(const TKey &p_key) {
+		return get_element(p_key);
+	}
+
+	/**
+	 * Same as get, except it can return NULL when item was not found.
+	 * This version is custom, will take a hash and a custom key (that should support operator==()
+	 */
+
+	template <class C>
+	_FORCE_INLINE_ TValue *custom_getptr(C p_custom_key, uint32_t p_custom_hash) {
+		if (unlikely(!elements)) {
+			return NULL;
+		}
+
+		const uint32_t capacity = hash_table_size_primes[capacity_index];
+		const uint64_t capacity_inv = hash_table_size_primes_inv[capacity_index];
+		uint32_t hash = p_custom_hash;
+		uint32_t pos = fastmod(hash, capacity_inv, capacity);
+		uint32_t distance = 0;
+
+		while (true) {
+			if (hashes[pos] == EMPTY_HASH) {
+				return NULL;
+			}
+
+			if (distance > _get_probe_length(pos, hashes[pos], capacity, capacity_inv)) {
+				return NULL;
+			}
+
+			if (hashes[pos] == hash && Comparator::compare(elements[pos]->data.key, p_custom_key)) {
+				return &elements[pos]->data.value;
+			}
+
+			pos = fastmod((pos + 1), capacity_inv, capacity);
+			distance++;
+		}
+
+		return NULL;
+	}
+
+	template <class C>
+	_FORCE_INLINE_ const TValue *custom_getptr(C p_custom_key, uint32_t p_custom_hash) const {
+		if (unlikely(!elements)) {
+			return NULL;
+		}
+
+		const uint32_t capacity = hash_table_size_primes[capacity_index];
+		const uint64_t capacity_inv = hash_table_size_primes_inv[capacity_index];
+		uint32_t hash = p_custom_hash;
+		uint32_t pos = fastmod(hash, capacity_inv, capacity);
+		uint32_t distance = 0;
+
+		while (true) {
+			if (hashes[pos] == EMPTY_HASH) {
+				return NULL;
+			}
+
+			if (distance > _get_probe_length(pos, hashes[pos], capacity, capacity_inv)) {
+				return NULL;
+			}
+
+			if (hashes[pos] == hash && Comparator::compare(elements[pos]->data.key, p_custom_key)) {
+				return &elements[pos]->data.value;
+			}
+
+			pos = fastmod((pos + 1), capacity_inv, capacity);
+			distance++;
+		}
+
+		return NULL;
+	}
+
+	_FORCE_INLINE_ bool has(const TKey &p_key) const {
+		uint32_t _pos = 0;
+		return _lookup_pos(p_key, _pos);
+	}
+
+	bool erase(const TKey &p_key) {
+		uint32_t pos = 0;
+		bool exists = _lookup_pos(p_key, pos);
+
+		if (!exists) {
+			return false;
+		}
+
+		const uint32_t capacity = hash_table_size_primes[capacity_index];
+		const uint64_t capacity_inv = hash_table_size_primes_inv[capacity_index];
+		uint32_t next_pos = fastmod((pos + 1), capacity_inv, capacity);
+		while (hashes[next_pos] != EMPTY_HASH && _get_probe_length(next_pos, hashes[next_pos], capacity, capacity_inv) != 0) {
+			SWAP(hashes[next_pos], hashes[pos]);
+			SWAP(elements[next_pos], elements[pos]);
+			pos = next_pos;
+			next_pos = fastmod((pos + 1), capacity_inv, capacity);
+		}
+
+		hashes[pos] = EMPTY_HASH;
+
+		if (head_element == elements[pos]) {
+			head_element = elements[pos]->next;
+		}
+
+		if (tail_element == elements[pos]) {
+			tail_element = elements[pos]->prev;
+		}
+
+		if (elements[pos]->prev) {
+			elements[pos]->prev->next = elements[pos]->next;
+		}
+
+		if (elements[pos]->next) {
+			elements[pos]->next->prev = elements[pos]->prev;
+		}
+
+		memdelete(elements[pos]);
+		elements[pos] = nullptr;
+
+		num_elements--;
+		return true;
+	}
+
+	// Reserves space for a number of elements, useful to avoid many resizes and rehashes.
+	// If adding a known (possibly large) number of elements at once, must be larger than old capacity.
+	void reserve(uint32_t p_new_capacity) {
+		uint32_t new_index = capacity_index;
+
+		while (hash_table_size_primes[new_index] < p_new_capacity) {
+			ERR_FAIL_COND_MSG(new_index + 1 == (uint32_t)HASH_TABLE_SIZE_MAX, nullptr);
+			new_index++;
+		}
+
+		if (new_index == capacity_index) {
+			return;
+		}
+
+		if (elements == nullptr) {
+			capacity_index = new_index;
+			return; // Unallocated yet.
+		}
+		_resize_and_rehash(new_index);
+	}
+
+	_FORCE_INLINE_ Element *front() {
+		return head_element;
+	}
+	_FORCE_INLINE_ Element *back() {
+		return tail_element;
+	}
+
+	_FORCE_INLINE_ const Element *front() const {
+		return head_element;
+	}
+	_FORCE_INLINE_ const Element *back() const {
+		return tail_element;
+	}
+
+	/**
+	 * Get the next key to p_key, and the first key if p_key is null.
+	 * Returns a pointer to the next key if found, NULL otherwise.
+	 * Adding/Removing elements while iterating will, of course, have unexpected results, don't do it.
+	 *
+	 * Example:
+	 *
+	 * 	const TKey *k=NULL;
+	 *
+	 * 	while( (k=table.next(k)) ) {
+	 *
+	 * 		print( *k );
+	 * 	}
+	 *
+	 * This is for backwards compatibility. Use this syntax instead for new code:
+	 *
+	 * for (const HashMap<K, V>::Element *E = map.front(); E; E = E->next) {
+	 *     ...
+	 * }
+	 *
+	 */
+	const TKey *next(const TKey *p_key) const {
+		if (unlikely(!elements)) {
+			return nullptr;
+		}
+
+		if (!p_key) { /* get the first key */
+
+			if (unlikely(!front())) {
+				return nullptr;
+			}
+
+			return &front()->data.key;
+
+		} else { /* get the next key */
+
+			const Element *e = get_element(*p_key);
+			ERR_FAIL_COND_V_MSG(!e, nullptr, "Invalid key supplied.");
+			if (e->next) {
+				/* if there is a "next" in the list, return that */
+				return &e->next->data.key;
+			}
+
+			/* nothing found, was at end */
+		}
+
+		return nullptr; /* nothing found */
+	}
+
+	/* Indexing */
+
+	const TValue &operator[](const TKey &p_key) const {
+		uint32_t pos = 0;
+		bool exists = _lookup_pos(p_key, pos);
+		CRASH_COND(!exists);
+		return elements[pos]->data.value;
+	}
+
+	TValue &operator[](const TKey &p_key) {
+		uint32_t pos = 0;
+		bool exists = _lookup_pos(p_key, pos);
+		if (!exists) {
+			return _insert(p_key, TValue())->data.value;
+		} else {
+			return elements[pos]->data.value;
+		}
+	}
+
+	/* Insert */
+
+	Element *insert(const TKey &p_key, const TValue &p_value, bool p_front_insert = false) {
+		return _insert(p_key, p_value, p_front_insert);
+	}
+
+	Element *set(const TKey &p_key, const TValue &p_value, bool p_front_insert = false) {
+		return _insert(p_key, p_value, p_front_insert);
+	}
+
+	/* Helpers */
+
+	void get_key_list(List<TKey> *p_keys) const {
+		if (unlikely(!elements)) {
+			return;
+		}
+
+		for (const Element *E = front(); E; E = E->next) {
+			p_keys->push_back(E->data.key);
+		}
+	}
+
+	/* Constructors */
+
+	HashMap(const HashMap &p_other) {
+		reserve(hash_table_size_primes[p_other.capacity_index]);
+
+		if (p_other.num_elements == 0) {
+			return;
+		}
+
+		for (const Element *E = p_other.front(); E; E = E->next) {
+			insert(E->data.key, E->data.value);
+		}
+	}
+
+	void operator=(const HashMap &p_other) {
+		if (this == &p_other) {
+			return; // Ignore self assignment.
+		}
+		if (num_elements != 0) {
+			clear();
+		}
+
+		reserve(hash_table_size_primes[p_other.capacity_index]);
+
+		if (p_other.elements == nullptr) {
+			return; // Nothing to copy.
+		}
+
+		for (const Element *E = p_other.front(); E; E = E->next) {
+			insert(E->data.key, E->data.value);
+		}
+	}
+
+	HashMap(uint32_t p_initial_capacity) {
+		// Capacity can't be 0.
+		capacity_index = 0;
+		reserve(p_initial_capacity);
+	}
+	HashMap() {
+		capacity_index = MIN_CAPACITY_INDEX;
+	}
+
+	uint32_t debug_get_hash(uint32_t p_index) {
+		if (num_elements == 0) {
+			return 0;
+		}
+		ERR_FAIL_INDEX_V(p_index, get_capacity(), 0);
+		return hashes[p_index];
+	}
+	Element *debug_get_element(uint32_t p_index) {
+		if (num_elements == 0) {
+			return NULL;
+		}
+
+		ERR_FAIL_INDEX_V(p_index, get_capacity(), NULL);
+
+		return elements[p_index];
+	}
+
+	~HashMap() {
+		clear();
+
+		if (elements != nullptr) {
+			Memory::free_static(elements);
+			Memory::free_static(hashes);
+		}
+	}
+
+private:
+	Element **elements = nullptr;
+	uint32_t *hashes = nullptr;
+	Element *head_element = nullptr;
+	Element *tail_element = nullptr;
+
+	uint32_t capacity_index = 0;
+	uint32_t num_elements = 0;
+
+	_FORCE_INLINE_ uint32_t _hash(const TKey &p_key) const {
+		uint32_t hash = Hasher::hash(p_key);
+
+		if (unlikely(hash == EMPTY_HASH)) {
+			hash = EMPTY_HASH + 1;
+		}
+
+		return hash;
+	}
+
+	static _FORCE_INLINE_ uint32_t _get_probe_length(const uint32_t p_pos, const uint32_t p_hash, const uint32_t p_capacity, const uint64_t p_capacity_inv) {
+		const uint32_t original_pos = fastmod(p_hash, p_capacity_inv, p_capacity);
+		return fastmod(p_pos - original_pos + p_capacity, p_capacity_inv, p_capacity);
+	}
+
+	bool _lookup_pos(const TKey &p_key, uint32_t &r_pos) const {
+		if (elements == nullptr || num_elements == 0) {
+			return false; // Failed lookups, no elements
+		}
+
+		const uint32_t capacity = hash_table_size_primes[capacity_index];
+		const uint64_t capacity_inv = hash_table_size_primes_inv[capacity_index];
+		uint32_t hash = _hash(p_key);
+		uint32_t pos = fastmod(hash, capacity_inv, capacity);
+		uint32_t distance = 0;
+
+		while (true) {
+			if (hashes[pos] == EMPTY_HASH) {
+				return false;
+			}
+
+			if (distance > _get_probe_length(pos, hashes[pos], capacity, capacity_inv)) {
+				return false;
+			}
+
+			if (hashes[pos] == hash && Comparator::compare(elements[pos]->data.key, p_key)) {
+				r_pos = pos;
+				return true;
+			}
+
+			pos = fastmod((pos + 1), capacity_inv, capacity);
+			distance++;
+		}
+	}
+
+	void _insert_with_hash(uint32_t p_hash, Element *p_value) {
+		const uint32_t capacity = hash_table_size_primes[capacity_index];
+		const uint64_t capacity_inv = hash_table_size_primes_inv[capacity_index];
+		uint32_t hash = p_hash;
+		Element *value = p_value;
+		uint32_t distance = 0;
+		uint32_t pos = fastmod(hash, capacity_inv, capacity);
+
+		while (true) {
+			if (hashes[pos] == EMPTY_HASH) {
+				elements[pos] = value;
+				hashes[pos] = hash;
+
+				num_elements++;
+
+				return;
+			}
+
+			// Not an empty slot, let's check the probing length of the existing one.
+			uint32_t existing_probe_len = _get_probe_length(pos, hashes[pos], capacity, capacity_inv);
+			if (existing_probe_len < distance) {
+				SWAP(hash, hashes[pos]);
+				SWAP(value, elements[pos]);
+				distance = existing_probe_len;
+			}
+
+			pos = fastmod((pos + 1), capacity_inv, capacity);
+			distance++;
+		}
+	}
+
+	void _resize_and_rehash(uint32_t p_new_capacity_index) {
+		uint32_t old_capacity = hash_table_size_primes[capacity_index];
+
+		// Capacity can't be 0.
+		capacity_index = MAX((uint32_t)MIN_CAPACITY_INDEX, p_new_capacity_index);
+
+		uint32_t capacity = hash_table_size_primes[capacity_index];
+
+		Element **old_elements = elements;
+		uint32_t *old_hashes = hashes;
+
+		num_elements = 0;
+		hashes = reinterpret_cast<uint32_t *>(Memory::alloc_static(sizeof(uint32_t) * capacity));
+		elements = reinterpret_cast<Element **>(Memory::alloc_static(sizeof(Element *) * capacity));
+
+		for (uint32_t i = 0; i < capacity; i++) {
+			hashes[i] = 0;
+			elements[i] = nullptr;
+		}
+
+		if (old_capacity == 0) {
+			// Nothing to do.
+			return;
+		}
+
+		for (uint32_t i = 0; i < old_capacity; i++) {
+			if (old_hashes[i] == EMPTY_HASH) {
+				continue;
+			}
+
+			_insert_with_hash(old_hashes[i], old_elements[i]);
+		}
+
+		Memory::free_static(old_elements);
+		Memory::free_static(old_hashes);
+	}
+
+	_FORCE_INLINE_ Element *_insert(const TKey &p_key, const TValue &p_value, bool p_front_insert = false) {
+		uint32_t capacity = hash_table_size_primes[capacity_index];
+		if (unlikely(elements == nullptr)) {
+			// Allocate on demand to save memory.
+
+			hashes = reinterpret_cast<uint32_t *>(Memory::alloc_static(sizeof(uint32_t) * capacity));
+			elements = reinterpret_cast<Element **>(Memory::alloc_static(sizeof(Element *) * capacity));
+
+			for (uint32_t i = 0; i < capacity; i++) {
+				hashes[i] = EMPTY_HASH;
+				elements[i] = nullptr;
+			}
+		}
+
+		uint32_t pos = 0;
+		bool exists = _lookup_pos(p_key, pos);
+
+		if (exists) {
+			elements[pos]->data.value = p_value;
+			return elements[pos];
+		} else {
+			if (num_elements + 1 > MAX_OCCUPANCY * capacity) {
+				ERR_FAIL_COND_V_MSG(capacity_index + 1 == HASH_TABLE_SIZE_MAX, nullptr, "Hash table maximum capacity reached, aborting insertion.");
+				_resize_and_rehash(capacity_index + 1);
+			}
+
+			Element *elem = memnew(Element(p_key, p_value));
+
+			if (tail_element == nullptr) {
+				head_element = elem;
+				tail_element = elem;
+			} else if (p_front_insert) {
+				head_element->prev = elem;
+				elem->next = head_element;
+				head_element = elem;
+			} else {
+				tail_element->next = elem;
+				elem->prev = tail_element;
+				tail_element = elem;
+			}
+
+			uint32_t hash = _hash(p_key);
+			_insert_with_hash(hash, elem);
+			return elem;
+		}
+	}
+};
+
+#endif // HASH_MAP_H

core/containers/hash_set.h

@@ -0,0 +1,505 @@
+/*************************************************************************/
+/*  hash_set.h                                                           */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef HASH_SET_H
+#define HASH_SET_H
+
+#include "core/containers/hash_map.h"
+#include "core/containers/hashfuncs.h"
+#include "core/math/math_funcs.h"
+#include "core/os/memory.h"
+
+/**
+ * Implementation of Set using a bidi indexed hash map.
+ * Use RBSet instead of this only if the following conditions are met:
+ *
+ * - You need to keep an iterator or const pointer to Key and you intend to add/remove elements in the meantime.
+ * - Iteration order does matter (via operator<)
+ *
+ */
+
+template <class TKey,
+		class Hasher = HashMapHasherDefault,
+		class Comparator = HashMapComparatorDefault<TKey>>
+class HashSet {
+public:
+	static constexpr uint32_t MIN_CAPACITY_INDEX = 2; // Use a prime.
+	static constexpr float MAX_OCCUPANCY = 0.75;
+	static constexpr uint32_t EMPTY_HASH = 0;
+
+private:
+	TKey *keys = nullptr;
+	uint32_t *hash_to_key = nullptr;
+	uint32_t *key_to_hash = nullptr;
+	uint32_t *hashes = nullptr;
+
+	uint32_t capacity_index = 0;
+	uint32_t num_elements = 0;
+
+	_FORCE_INLINE_ uint32_t _hash(const TKey &p_key) const {
+		uint32_t hash = Hasher::hash(p_key);
+
+		if (unlikely(hash == EMPTY_HASH)) {
+			hash = EMPTY_HASH + 1;
+		}
+
+		return hash;
+	}
+
+	static _FORCE_INLINE_ uint32_t _get_probe_length(const uint32_t p_pos, const uint32_t p_hash, const uint32_t p_capacity, const uint64_t p_capacity_inv) {
+		const uint32_t original_pos = fastmod(p_hash, p_capacity_inv, p_capacity);
+		return fastmod(p_pos - original_pos + p_capacity, p_capacity_inv, p_capacity);
+	}
+
+	bool _lookup_pos(const TKey &p_key, uint32_t &r_pos) const {
+		if (keys == nullptr || num_elements == 0) {
+			return false; // Failed lookups, no elements
+		}
+
+		const uint32_t capacity = hash_table_size_primes[capacity_index];
+		const uint64_t capacity_inv = hash_table_size_primes_inv[capacity_index];
+		uint32_t hash = _hash(p_key);
+		uint32_t pos = fastmod(hash, capacity_inv, capacity);
+		uint32_t distance = 0;
+
+		while (true) {
+			if (hashes[pos] == EMPTY_HASH) {
+				return false;
+			}
+
+			if (distance > _get_probe_length(pos, hashes[pos], capacity, capacity_inv)) {
+				return false;
+			}
+
+			if (hashes[pos] == hash && Comparator::compare(keys[hash_to_key[pos]], p_key)) {
+				r_pos = hash_to_key[pos];
+				return true;
+			}
+
+			pos = fastmod(pos + 1, capacity_inv, capacity);
+			distance++;
+		}
+	}
+
+	uint32_t _insert_with_hash(uint32_t p_hash, uint32_t p_index) {
+		const uint32_t capacity = hash_table_size_primes[capacity_index];
+		const uint64_t capacity_inv = hash_table_size_primes_inv[capacity_index];
+		uint32_t hash = p_hash;
+		uint32_t index = p_index;
+		uint32_t distance = 0;
+		uint32_t pos = fastmod(hash, capacity_inv, capacity);
+
+		while (true) {
+			if (hashes[pos] == EMPTY_HASH) {
+				hashes[pos] = hash;
+				key_to_hash[index] = pos;
+				hash_to_key[pos] = index;
+				return pos;
+			}
+
+			// Not an empty slot, let's check the probing length of the existing one.
+			uint32_t existing_probe_len = _get_probe_length(pos, hashes[pos], capacity, capacity_inv);
+			if (existing_probe_len < distance) {
+				key_to_hash[index] = pos;
+				SWAP(hash, hashes[pos]);
+				SWAP(index, hash_to_key[pos]);
+				distance = existing_probe_len;
+			}
+
+			pos = fastmod(pos + 1, capacity_inv, capacity);
+			distance++;
+		}
+	}
+
+	void _resize_and_rehash(uint32_t p_new_capacity_index) {
+		// Capacity can't be 0.
+		capacity_index = MAX((uint32_t)MIN_CAPACITY_INDEX, p_new_capacity_index);
+
+		uint32_t capacity = hash_table_size_primes[capacity_index];
+
+		uint32_t *old_hashes = hashes;
+		uint32_t *old_key_to_hash = key_to_hash;
+
+		hashes = reinterpret_cast<uint32_t *>(Memory::alloc_static(sizeof(uint32_t) * capacity));
+		keys = reinterpret_cast<TKey *>(Memory::realloc_static(keys, sizeof(TKey) * capacity));
+		key_to_hash = reinterpret_cast<uint32_t *>(Memory::alloc_static(sizeof(uint32_t) * capacity));
+		hash_to_key = reinterpret_cast<uint32_t *>(Memory::realloc_static(hash_to_key, sizeof(uint32_t) * capacity));
+
+		for (uint32_t i = 0; i < capacity; i++) {
+			hashes[i] = EMPTY_HASH;
+		}
+
+		for (uint32_t i = 0; i < num_elements; i++) {
+			uint32_t h = old_hashes[old_key_to_hash[i]];
+			_insert_with_hash(h, i);
+		}
+
+		Memory::free_static(old_hashes);
+		Memory::free_static(old_key_to_hash);
+	}
+
+	_FORCE_INLINE_ int32_t _insert(const TKey &p_key) {
+		uint32_t capacity = hash_table_size_primes[capacity_index];
+		if (unlikely(keys == nullptr)) {
+			// Allocate on demand to save memory.
+
+			hashes = reinterpret_cast<uint32_t *>(Memory::alloc_static(sizeof(uint32_t) * capacity));
+			keys = reinterpret_cast<TKey *>(Memory::alloc_static(sizeof(TKey) * capacity));
+			key_to_hash = reinterpret_cast<uint32_t *>(Memory::alloc_static(sizeof(uint32_t) * capacity));
+			hash_to_key = reinterpret_cast<uint32_t *>(Memory::alloc_static(sizeof(uint32_t) * capacity));
+
+			for (uint32_t i = 0; i < capacity; i++) {
+				hashes[i] = EMPTY_HASH;
+			}
+		}
+
+		uint32_t pos = 0;
+		bool exists = _lookup_pos(p_key, pos);
+
+		if (exists) {
+			return pos;
+		} else {
+			if (num_elements + 1 > MAX_OCCUPANCY * capacity) {
+				ERR_FAIL_COND_V_MSG(capacity_index + 1 == HASH_TABLE_SIZE_MAX, -1, "Hash table maximum capacity reached, aborting insertion.");
+				_resize_and_rehash(capacity_index + 1);
+			}
+
+			uint32_t hash = _hash(p_key);
+			memnew_placement(&keys[num_elements], TKey(p_key));
+			_insert_with_hash(hash, num_elements);
+			num_elements++;
+			return num_elements - 1;
+		}
+	}
+
+	void _init_from(const HashSet &p_other) {
+		capacity_index = p_other.capacity_index;
+		num_elements = p_other.num_elements;
+
+		if (p_other.num_elements == 0) {
+			return;
+		}
+
+		uint32_t capacity = hash_table_size_primes[capacity_index];
+
+		hashes = reinterpret_cast<uint32_t *>(Memory::alloc_static(sizeof(uint32_t) * capacity));
+		keys = reinterpret_cast<TKey *>(Memory::alloc_static(sizeof(TKey) * capacity));
+		key_to_hash = reinterpret_cast<uint32_t *>(Memory::alloc_static(sizeof(uint32_t) * capacity));
+		hash_to_key = reinterpret_cast<uint32_t *>(Memory::alloc_static(sizeof(uint32_t) * capacity));
+
+		for (uint32_t i = 0; i < num_elements; i++) {
+			memnew_placement(&keys[i], TKey(p_other.keys[i]));
+			key_to_hash[i] = p_other.key_to_hash[i];
+		}
+
+		for (uint32_t i = 0; i < capacity; i++) {
+			hashes[i] = p_other.hashes[i];
+			hash_to_key[i] = p_other.hash_to_key[i];
+		}
+	}
+
+public:
+	_FORCE_INLINE_ uint32_t get_capacity() const { return hash_table_size_primes[capacity_index]; }
+	_FORCE_INLINE_ uint32_t size() const { return num_elements; }
+
+	/* Standard Godot Container API */
+
+	bool is_empty() const {
+		return num_elements == 0;
+	}
+
+	void clear() {
+		if (keys == nullptr || num_elements == 0) {
+			return;
+		}
+		uint32_t capacity = hash_table_size_primes[capacity_index];
+		for (uint32_t i = 0; i < capacity; i++) {
+			hashes[i] = EMPTY_HASH;
+		}
+		for (uint32_t i = 0; i < num_elements; i++) {
+			keys[i].~TKey();
+		}
+
+		num_elements = 0;
+	}
+
+	_FORCE_INLINE_ bool has(const TKey &p_key) const {
+		uint32_t _pos = 0;
+		return _lookup_pos(p_key, _pos);
+	}
+
+	bool erase(const TKey &p_key) {
+		uint32_t pos = 0;
+		bool exists = _lookup_pos(p_key, pos);
+
+		if (!exists) {
+			return false;
+		}
+
+		uint32_t key_pos = pos;
+		pos = key_to_hash[pos]; //make hash pos
+
+		const uint32_t capacity = hash_table_size_primes[capacity_index];
+		const uint64_t capacity_inv = hash_table_size_primes_inv[capacity_index];
+		uint32_t next_pos = fastmod(pos + 1, capacity_inv, capacity);
+		while (hashes[next_pos] != EMPTY_HASH && _get_probe_length(next_pos, hashes[next_pos], capacity, capacity_inv) != 0) {
+			uint32_t kpos = hash_to_key[pos];
+			uint32_t kpos_next = hash_to_key[next_pos];
+			SWAP(key_to_hash[kpos], key_to_hash[kpos_next]);
+			SWAP(hashes[next_pos], hashes[pos]);
+			SWAP(hash_to_key[next_pos], hash_to_key[pos]);
+
+			pos = next_pos;
+			next_pos = fastmod(pos + 1, capacity_inv, capacity);
+		}
+
+		hashes[pos] = EMPTY_HASH;
+		keys[key_pos].~TKey();
+		num_elements--;
+		if (key_pos < num_elements) {
+			// Not the last key, move the last one here to keep keys lineal
+			memnew_placement(&keys[key_pos], TKey(keys[num_elements]));
+			keys[num_elements].~TKey();
+			key_to_hash[key_pos] = key_to_hash[num_elements];
+			hash_to_key[key_to_hash[num_elements]] = key_pos;
+		}
+
+		return true;
+	}
+
+	// Reserves space for a number of elements, useful to avoid many resizes and rehashes.
+	// If adding a known (possibly large) number of elements at once, must be larger than old capacity.
+	void reserve(uint32_t p_new_capacity) {
+		uint32_t new_index = capacity_index;
+
+		while (hash_table_size_primes[new_index] < p_new_capacity) {
+			ERR_FAIL_COND_MSG(new_index + 1 == (uint32_t)HASH_TABLE_SIZE_MAX, nullptr);
+			new_index++;
+		}
+
+		if (new_index == capacity_index) {
+			return;
+		}
+
+		if (keys == nullptr) {
+			capacity_index = new_index;
+			return; // Unallocated yet.
+		}
+		_resize_and_rehash(new_index);
+	}
+
+	/** Iterator API **/
+
+	struct Iterator {
+		_FORCE_INLINE_ const TKey &operator*() const {
+			return keys[index];
+		}
+		_FORCE_INLINE_ const TKey *operator->() const {
+			return &keys[index];
+		}
+		_FORCE_INLINE_ Iterator &operator++() {
+			index++;
+			if (index >= (int32_t)num_keys) {
+				index = -1;
+				keys = nullptr;
+				num_keys = 0;
+			}
+			return *this;
+		}
+		_FORCE_INLINE_ Iterator &operator--() {
+			index--;
+			if (index < 0) {
+				index = -1;
+				keys = nullptr;
+				num_keys = 0;
+			}
+			return *this;
+		}
+
+		_FORCE_INLINE_ const TKey &key() const {
+			return keys[index];
+		}
+		_FORCE_INLINE_ const TKey *key_ptr() const {
+			return &keys[index];
+		}
+
+		_FORCE_INLINE_ Iterator &next() {
+			index++;
+			if (index >= (int32_t)num_keys) {
+				index = -1;
+				keys = nullptr;
+				num_keys = 0;
+			}
+			return *this;
+		}
+		_FORCE_INLINE_ Iterator &prev() {
+			index--;
+			if (index < 0) {
+				index = -1;
+				keys = nullptr;
+				num_keys = 0;
+			}
+			return *this;
+		}
+
+		_FORCE_INLINE_ bool valid() const {
+			return keys != nullptr;
+		}
+
+		_FORCE_INLINE_ bool operator==(const Iterator &b) const { return keys == b.keys && index == b.index; }
+		_FORCE_INLINE_ bool operator!=(const Iterator &b) const { return keys != b.keys || index != b.index; }
+
+		_FORCE_INLINE_ explicit operator bool() const {
+			return keys != nullptr;
+		}
+
+		_FORCE_INLINE_ Iterator(const TKey *p_keys, uint32_t p_num_keys, int32_t p_index = -1) {
+			keys = p_keys;
+			num_keys = p_num_keys;
+			index = p_index;
+		}
+		_FORCE_INLINE_ Iterator() {}
+		_FORCE_INLINE_ Iterator(const Iterator &p_it) {
+			keys = p_it.keys;
+			num_keys = p_it.num_keys;
+			index = p_it.index;
+		}
+		_FORCE_INLINE_ void operator=(const Iterator &p_it) {
+			keys = p_it.keys;
+			num_keys = p_it.num_keys;
+			index = p_it.index;
+		}
+
+	private:
+		const TKey *keys = nullptr;
+		uint32_t num_keys = 0;
+		int32_t index = -1;
+	};
+
+	_FORCE_INLINE_ Iterator begin() const {
+		return num_elements ? Iterator(keys, num_elements, 0) : Iterator();
+	}
+	_FORCE_INLINE_ Iterator end() const {
+		return Iterator();
+	}
+	_FORCE_INLINE_ Iterator last() const {
+		if (num_elements == 0) {
+			return Iterator();
+		}
+		return Iterator(keys, num_elements, num_elements - 1);
+	}
+
+	_FORCE_INLINE_ Iterator find(const TKey &p_key) const {
+		uint32_t pos = 0;
+		bool exists = _lookup_pos(p_key, pos);
+		if (!exists) {
+			return end();
+		}
+		return Iterator(keys, num_elements, pos);
+	}
+
+	_FORCE_INLINE_ void remove(const Iterator &p_iter) {
+		if (p_iter) {
+			erase(*p_iter);
+		}
+	}
+
+	/* Insert */
+
+	Iterator insert(const TKey &p_key) {
+		uint32_t pos = _insert(p_key);
+		return Iterator(keys, num_elements, pos);
+	}
+
+	/* Constructors */
+
+	HashSet(const HashSet &p_other) {
+		_init_from(p_other);
+	}
+
+	void operator=(const HashSet &p_other) {
+		if (this == &p_other) {
+			return; // Ignore self assignment.
+		}
+
+		clear();
+
+		if (keys != nullptr) {
+			Memory::free_static(keys);
+			Memory::free_static(key_to_hash);
+			Memory::free_static(hash_to_key);
+			Memory::free_static(hashes);
+			keys = nullptr;
+			hashes = nullptr;
+			hash_to_key = nullptr;
+			key_to_hash = nullptr;
+		}
+
+		_init_from(p_other);
+	}
+
+	HashSet(uint32_t p_initial_capacity) {
+		// Capacity can't be 0.
+		capacity_index = 0;
+		reserve(p_initial_capacity);
+	}
+	HashSet() {
+		capacity_index = MIN_CAPACITY_INDEX;
+	}
+
+	void reset() {
+		clear();
+
+		if (keys != nullptr) {
+			Memory::free_static(keys);
+			Memory::free_static(key_to_hash);
+			Memory::free_static(hash_to_key);
+			Memory::free_static(hashes);
+			keys = nullptr;
+			hashes = nullptr;
+			hash_to_key = nullptr;
+			key_to_hash = nullptr;
+		}
+		capacity_index = MIN_CAPACITY_INDEX;
+	}
+
+	~HashSet() {
+		clear();
+
+		if (keys != nullptr) {
+			Memory::free_static(keys);
+			Memory::free_static(key_to_hash);
+			Memory::free_static(hash_to_key);
+			Memory::free_static(hashes);
+		}
+	}
+};
+
+#endif // HASH_SET_H

core/containers/hashfuncs.h

@@ -0,0 +1,530 @@
+#ifndef HASHFUNCS_H
+#define HASHFUNCS_H
+/*************************************************************************/
+/*  hashfuncs.h                                                          */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "core/math/aabb.h"
+#include "core/math/math_defs.h"
+#include "core/math/math_funcs.h"
+#include "core/math/rect2.h"
+#include "core/math/rect2i.h"
+#include "core/math/vector2.h"
+#include "core/math/vector2i.h"
+#include "core/math/vector3.h"
+#include "core/math/vector3i.h"
+#include "core/math/vector4.h"
+#include "core/math/vector4i.h"
+#include "core/object/object_id.h"
+#include "core/string/node_path.h"
+#include "core/string/string_name.h"
+#include "core/string/ustring.h"
+#include "core/containers/rid.h"
+#include "core/typedefs.h"
+
+/**
+ * Hashing functions
+ */
+
+/**
+ * DJB2 Hash function
+ * @param C String
+ * @return 32-bits hashcode
+ */
+static inline uint32_t hash_djb2(const char *p_cstr) {
+	const unsigned char *chr = (const unsigned char *)p_cstr;
+	uint32_t hash = 5381;
+	uint32_t c;
+
+	while ((c = *chr++)) {
+		hash = ((hash << 5) + hash) ^ c; /* hash * 33 ^ c */
+	}
+
+	return hash;
+}
+
+static inline uint32_t hash_djb2_buffer(const uint8_t *p_buff, int p_len, uint32_t p_prev = 5381) {
+	uint32_t hash = p_prev;
+
+	for (int i = 0; i < p_len; i++) {
+		hash = ((hash << 5) + hash) ^ p_buff[i]; /* hash * 33 ^ c */
+	}
+
+	return hash;
+}
+
+static inline uint32_t hash_djb2_one_32(uint32_t p_in, uint32_t p_prev = 5381) {
+	return ((p_prev << 5) + p_prev) ^ p_in;
+}
+
+/**
+ * Thomas Wang's 64-bit to 32-bit Hash function:
+ * https://web.archive.org/web/20071223173210/https:/www.concentric.net/~Ttwang/tech/inthash.htm
+ *
+ * @param p_int - 64-bit unsigned integer key to be hashed
+ * @return unsigned 32-bit value representing hashcode
+ */
+static inline uint32_t hash_one_uint64(const uint64_t p_int) {
+	uint64_t v = p_int;
+	v = (~v) + (v << 18); // v = (v << 18) - v - 1;
+	v = v ^ (v >> 31);
+	v = v * 21; // v = (v + (v << 2)) + (v << 4);
+	v = v ^ (v >> 11);
+	v = v + (v << 6);
+	v = v ^ (v >> 22);
+	return (uint32_t)v;
+}
+
+#define HASH_MURMUR3_SEED 0x7F07C65
+// Murmurhash3 32-bit version.
+// All MurmurHash versions are public domain software, and the author disclaims all copyright to their code.
+
+static _FORCE_INLINE_ uint32_t hash_murmur3_one_32(uint32_t p_in, uint32_t p_seed = HASH_MURMUR3_SEED) {
+	p_in *= 0xcc9e2d51;
+	p_in = (p_in << 15) | (p_in >> 17);
+	p_in *= 0x1b873593;
+
+	p_seed ^= p_in;
+	p_seed = (p_seed << 13) | (p_seed >> 19);
+	p_seed = p_seed * 5 + 0xe6546b64;
+
+	return p_seed;
+}
+
+static _FORCE_INLINE_ uint32_t hash_murmur3_one_float(float p_in, uint32_t p_seed = HASH_MURMUR3_SEED) {
+	union {
+		float f;
+		uint32_t i;
+	} u;
+
+	// Normalize +/- 0.0 and NaN values so they hash the same.
+	if (p_in == 0.0f) {
+		u.f = 0.0;
+	} else if (Math::is_nan(p_in)) {
+		u.f = NAN;
+	} else {
+		u.f = p_in;
+	}
+
+	return hash_murmur3_one_32(u.i, p_seed);
+}
+
+static _FORCE_INLINE_ uint32_t hash_murmur3_one_64(uint64_t p_in, uint32_t p_seed = HASH_MURMUR3_SEED) {
+	p_seed = hash_murmur3_one_32(p_in & 0xFFFFFFFF, p_seed);
+	return hash_murmur3_one_32(p_in >> 32, p_seed);
+}
+
+static _FORCE_INLINE_ uint32_t hash_murmur3_one_double(double p_in, uint32_t p_seed = HASH_MURMUR3_SEED) {
+	union {
+		double d;
+		uint64_t i;
+	} u;
+
+	// Normalize +/- 0.0 and NaN values so they hash the same.
+	if (p_in == 0.0f) {
+		u.d = 0.0;
+	} else if (Math::is_nan(p_in)) {
+		u.d = NAN;
+	} else {
+		u.d = p_in;
+	}
+
+	return hash_murmur3_one_64(u.i, p_seed);
+}
+
+static _FORCE_INLINE_ uint32_t hash_murmur3_one_real(real_t p_in, uint32_t p_seed = HASH_MURMUR3_SEED) {
+#ifdef REAL_T_IS_DOUBLE
+	return hash_murmur3_one_double(p_in, p_seed);
+#else
+	return hash_murmur3_one_float(p_in, p_seed);
+#endif
+}
+
+static _FORCE_INLINE_ uint32_t hash_rotl32(uint32_t x, int8_t r) {
+	return (x << r) | (x >> (32 - r));
+}
+
+static _FORCE_INLINE_ uint32_t hash_fmix32(uint32_t h) {
+	h ^= h >> 16;
+	h *= 0x85ebca6b;
+	h ^= h >> 13;
+	h *= 0xc2b2ae35;
+	h ^= h >> 16;
+
+	return h;
+}
+
+static _FORCE_INLINE_ uint32_t hash_murmur3_buffer(const void *key, int length, const uint32_t seed = HASH_MURMUR3_SEED) {
+	// Although not required, this is a random prime number.
+	const uint8_t *data = (const uint8_t *)key;
+	const int nblocks = length / 4;
+
+	uint32_t h1 = seed;
+
+	const uint32_t c1 = 0xcc9e2d51;
+	const uint32_t c2 = 0x1b873593;
+
+	const uint32_t *blocks = (const uint32_t *)(data + nblocks * 4);
+
+	for (int i = -nblocks; i; i++) {
+		uint32_t k1 = blocks[i];
+
+		k1 *= c1;
+		k1 = hash_rotl32(k1, 15);
+		k1 *= c2;
+
+		h1 ^= k1;
+		h1 = hash_rotl32(h1, 13);
+		h1 = h1 * 5 + 0xe6546b64;
+	}
+
+	const uint8_t *tail = (const uint8_t *)(data + nblocks * 4);
+
+	uint32_t k1 = 0;
+
+	switch (length & 3) {
+		case 3:
+			k1 ^= tail[2] << 16;
+			FALLTHROUGH;
+		case 2:
+			k1 ^= tail[1] << 8;
+			FALLTHROUGH;
+		case 1:
+			k1 ^= tail[0];
+			k1 *= c1;
+			k1 = hash_rotl32(k1, 15);
+			k1 *= c2;
+			h1 ^= k1;
+	};
+
+	// Finalize with additional bit mixing.
+	h1 ^= length;
+	return hash_fmix32(h1);
+}
+
+static inline uint32_t hash_djb2_one_float(double p_in, uint32_t p_prev = 5381) {
+	union {
+		double d;
+		uint64_t i;
+	} u;
+
+	// Normalize +/- 0.0 and NaN values so they hash the same.
+	if (p_in == 0.0f) {
+		u.d = 0.0;
+	} else if (Math::is_nan(p_in)) {
+		u.d = Math_NAN;
+	} else {
+		u.d = p_in;
+	}
+
+	return ((p_prev << 5) + p_prev) + hash_one_uint64(u.i);
+}
+
+template <class T>
+static inline uint32_t make_uint32_t(T p_in) {
+	union {
+		T t;
+		uint32_t _u32;
+	} _u;
+	_u._u32 = 0;
+	_u.t = p_in;
+	return _u._u32;
+}
+
+static _FORCE_INLINE_ uint64_t hash_djb2_one_float_64(double p_in, uint64_t p_prev = 5381) {
+	union {
+		double d;
+		uint64_t i;
+	} u;
+
+	// Normalize +/- 0.0 and NaN values so they hash the same.
+	if (p_in == 0.0f) {
+		u.d = 0.0;
+	} else if (Math::is_nan(p_in)) {
+		u.d = NAN;
+	} else {
+		u.d = p_in;
+	}
+
+	return ((p_prev << 5) + p_prev) + u.i;
+}
+
+static _FORCE_INLINE_ uint64_t hash_djb2_one_64(uint64_t p_in, uint64_t p_prev = 5381) {
+	return ((p_prev << 5) + p_prev) ^ p_in;
+}
+
+template <class T>
+static _FORCE_INLINE_ uint64_t hash_make_uint64_t(T p_in) {
+	union {
+		T t;
+		uint64_t _u64;
+	} _u;
+	_u._u64 = 0; // in case p_in is smaller
+
+	_u.t = p_in;
+	return _u._u64;
+}
+
+template <class T>
+static inline uint64_t make_uint64_t(T p_in) {
+	union {
+		T t;
+		uint64_t _u64;
+	} _u;
+	_u._u64 = 0; // in case p_in is smaller
+
+	_u.t = p_in;
+	return _u._u64;
+}
+
+template <class T>
+class Ref;
+
+struct HashMapHasherDefault {
+	// Generic hash function for any type.
+	template <class T>
+	static _FORCE_INLINE_ uint32_t hash(const T *p_pointer) { return hash_one_uint64((uint64_t)p_pointer); }
+
+	template <class T>
+	static _FORCE_INLINE_ uint32_t hash(const Ref<T> &p_ref) { return hash_one_uint64((uint64_t)p_ref.operator->()); }
+
+	static _FORCE_INLINE_ uint32_t hash(const String &p_string) { return p_string.hash(); }
+	static _FORCE_INLINE_ uint32_t hash(const char *p_cstr) { return hash_djb2(p_cstr); }
+	static _FORCE_INLINE_ uint32_t hash(const wchar_t p_wchar) { return hash_fmix32(p_wchar); }
+	static _FORCE_INLINE_ uint32_t hash(const char16_t p_uchar) { return hash_fmix32(p_uchar); }
+	static _FORCE_INLINE_ uint32_t hash(const char32_t p_uchar) { return hash_fmix32(p_uchar); }
+	static _FORCE_INLINE_ uint32_t hash(const RID &p_rid) { return hash_one_uint64(p_rid.get_id()); }
+	static _FORCE_INLINE_ uint32_t hash(const StringName &p_string_name) { return p_string_name.hash(); }
+	static _FORCE_INLINE_ uint32_t hash(const NodePath &p_path) { return p_path.hash(); }
+	//static _FORCE_INLINE_ uint32_t hash(const ObjectID &p_id) { return hash_one_uint64(p_id); }
+
+	static _FORCE_INLINE_ uint32_t hash(const uint64_t p_int) { return hash_one_uint64(p_int); }
+	static _FORCE_INLINE_ uint32_t hash(const int64_t p_int) { return hash_one_uint64(p_int); }
+	static _FORCE_INLINE_ uint32_t hash(const float p_float) { return hash_murmur3_one_float(p_float); }
+	static _FORCE_INLINE_ uint32_t hash(const double p_double) { return hash_murmur3_one_double(p_double); }
+	static _FORCE_INLINE_ uint32_t hash(const uint32_t p_int) { return hash_fmix32(p_int); }
+	static _FORCE_INLINE_ uint32_t hash(const int32_t p_int) { return hash_fmix32(p_int); }
+	static _FORCE_INLINE_ uint32_t hash(const uint16_t p_int) { return hash_fmix32(p_int); }
+	static _FORCE_INLINE_ uint32_t hash(const int16_t p_int) { return hash_fmix32(p_int); }
+	static _FORCE_INLINE_ uint32_t hash(const uint8_t p_int) { return hash_fmix32(p_int); }
+	static _FORCE_INLINE_ uint32_t hash(const int8_t p_int) { return hash_fmix32(p_int); }
+	
+	static _FORCE_INLINE_ uint32_t hash(const Vector2i &p_vec) {
+		uint32_t h = hash_murmur3_one_32(p_vec.x);
+		h = hash_murmur3_one_32(p_vec.y, h);
+		return hash_fmix32(h);
+	}
+	static _FORCE_INLINE_ uint32_t hash(const Vector3i &p_vec) {
+		uint32_t h = hash_murmur3_one_32(p_vec.x);
+		h = hash_murmur3_one_32(p_vec.y, h);
+		h = hash_murmur3_one_32(p_vec.z, h);
+		return hash_fmix32(h);
+	}
+	static _FORCE_INLINE_ uint32_t hash(const Vector4i &p_vec) {
+		uint32_t h = hash_murmur3_one_32(p_vec.x);
+		h = hash_murmur3_one_32(p_vec.y, h);
+		h = hash_murmur3_one_32(p_vec.z, h);
+		h = hash_murmur3_one_32(p_vec.w, h);
+		return hash_fmix32(h);
+	}
+	static _FORCE_INLINE_ uint32_t hash(const Vector2 &p_vec) {
+		uint32_t h = hash_murmur3_one_real(p_vec.x);
+		h = hash_murmur3_one_real(p_vec.y, h);
+		return hash_fmix32(h);
+	}
+	static _FORCE_INLINE_ uint32_t hash(const Vector3 &p_vec) {
+		uint32_t h = hash_murmur3_one_real(p_vec.x);
+		h = hash_murmur3_one_real(p_vec.y, h);
+		h = hash_murmur3_one_real(p_vec.z, h);
+		return hash_fmix32(h);
+	}
+	static _FORCE_INLINE_ uint32_t hash(const Vector4 &p_vec) {
+		uint32_t h = hash_murmur3_one_real(p_vec.x);
+		h = hash_murmur3_one_real(p_vec.y, h);
+		h = hash_murmur3_one_real(p_vec.z, h);
+		h = hash_murmur3_one_real(p_vec.w, h);
+		return hash_fmix32(h);
+	}
+	static _FORCE_INLINE_ uint32_t hash(const Rect2i &p_rect) {
+		uint32_t h = hash_murmur3_one_32(p_rect.position.x);
+		h = hash_murmur3_one_32(p_rect.position.y, h);
+		h = hash_murmur3_one_32(p_rect.size.x, h);
+		h = hash_murmur3_one_32(p_rect.size.y, h);
+		return hash_fmix32(h);
+	}
+	static _FORCE_INLINE_ uint32_t hash(const Rect2 &p_rect) {
+		uint32_t h = hash_murmur3_one_real(p_rect.position.x);
+		h = hash_murmur3_one_real(p_rect.position.y, h);
+		h = hash_murmur3_one_real(p_rect.size.x, h);
+		h = hash_murmur3_one_real(p_rect.size.y, h);
+		return hash_fmix32(h);
+	}
+	static _FORCE_INLINE_ uint32_t hash(const AABB &p_aabb) {
+		uint32_t h = hash_murmur3_one_real(p_aabb.position.x);
+		h = hash_murmur3_one_real(p_aabb.position.y, h);
+		h = hash_murmur3_one_real(p_aabb.position.z, h);
+		h = hash_murmur3_one_real(p_aabb.size.x, h);
+		h = hash_murmur3_one_real(p_aabb.size.y, h);
+		h = hash_murmur3_one_real(p_aabb.size.z, h);
+		return hash_fmix32(h);
+	}
+};
+
+template <typename T>
+struct HashMapComparatorDefault {
+	static bool compare(const T &p_lhs, const T &p_rhs) {
+		return p_lhs == p_rhs;
+	}
+};
+
+template <>
+struct HashMapComparatorDefault<float> {
+	static bool compare(const float &p_lhs, const float &p_rhs) {
+		return (p_lhs == p_rhs) || (Math::is_nan(p_lhs) && Math::is_nan(p_rhs));
+	}
+};
+
+template <>
+struct HashMapComparatorDefault<double> {
+	static bool compare(const double &p_lhs, const double &p_rhs) {
+		return (p_lhs == p_rhs) || (Math::is_nan(p_lhs) && Math::is_nan(p_rhs));
+	}
+};
+
+template <>
+struct HashMapComparatorDefault<Vector2> {
+	static bool compare(const Vector2 &p_lhs, const Vector2 &p_rhs) {
+		return ((p_lhs.x == p_rhs.x) || (Math::is_nan(p_lhs.x) && Math::is_nan(p_rhs.x))) && ((p_lhs.y == p_rhs.y) || (Math::is_nan(p_lhs.y) && Math::is_nan(p_rhs.y)));
+	}
+};
+
+template <>
+struct HashMapComparatorDefault<Vector3> {
+	static bool compare(const Vector3 &p_lhs, const Vector3 &p_rhs) {
+		return ((p_lhs.x == p_rhs.x) || (Math::is_nan(p_lhs.x) && Math::is_nan(p_rhs.x))) && ((p_lhs.y == p_rhs.y) || (Math::is_nan(p_lhs.y) && Math::is_nan(p_rhs.y))) && ((p_lhs.z == p_rhs.z) || (Math::is_nan(p_lhs.z) && Math::is_nan(p_rhs.z)));
+	}
+};
+
+constexpr uint32_t HASH_TABLE_SIZE_MAX = 29;
+
+const uint32_t hash_table_size_primes[HASH_TABLE_SIZE_MAX] = {
+	5,
+	13,
+	23,
+	47,
+	97,
+	193,
+	389,
+	769,
+	1543,
+	3079,
+	6151,
+	12289,
+	24593,
+	49157,
+	98317,
+	196613,
+	393241,
+	786433,
+	1572869,
+	3145739,
+	6291469,
+	12582917,
+	25165843,
+	50331653,
+	100663319,
+	201326611,
+	402653189,
+	805306457,
+	1610612741,
+};
+
+// Computed with elem_i = UINT64_C (0 x FFFFFFFF FFFFFFFF ) / d_i + 1, where d_i is the i-th element of the above array.
+const uint64_t hash_table_size_primes_inv[HASH_TABLE_SIZE_MAX] = {
+	3689348814741910324,
+	1418980313362273202,
+	802032351030850071,
+	392483916461905354,
+	190172619316593316,
+	95578984837873325,
+	47420935922132524,
+	23987963684927896,
+	11955116055547344,
+	5991147799191151,
+	2998982941588287,
+	1501077717772769,
+	750081082979285,
+	375261795343686,
+	187625172388393,
+	93822606204624,
+	46909513691883,
+	23456218233098,
+	11728086747027,
+	5864041509391,
+	2932024948977,
+	1466014921160,
+	733007198436,
+	366503839517,
+	183251896093,
+	91625960335,
+	45812983922,
+	22906489714,
+	11453246088
+};
+
+/**
+ * Fastmod computes ( n mod d ) given the precomputed c much faster than n % d.
+ * The implementation of fastmod is based on the following paper by Daniel Lemire et al.
+ * Faster Remainder by Direct Computation: Applications to Compilers and Software Libraries
+ * https://arxiv.org/abs/1902.01961
+ */
+static _FORCE_INLINE_ uint32_t fastmod(const uint32_t n, const uint64_t c, const uint32_t d) {
+#if defined(_MSC_VER)
+	// Returns the upper 64 bits of the product of two 64-bit unsigned integers.
+	// This intrinsic function is required since MSVC does not support unsigned 128-bit integers.
+#if defined(_M_X64) || defined(_M_ARM64)
+	return __umulh(c * n, d);
+#else
+	// Fallback to the slower method for 32-bit platforms.
+	return n % d;
+#endif // _M_X64 || _M_ARM64
+#else
+#ifdef __SIZEOF_INT128__
+	// Prevent compiler warning, because we know what we are doing.
+	uint64_t lowbits = c * n;
+	__extension__ typedef unsigned __int128 uint128;
+	return static_cast<uint64_t>(((uint128)lowbits * d) >> 64);
+#else
+	// Fallback to the slower method if no 128-bit unsigned integer type is available.
+	return n % d;
+#endif // __SIZEOF_INT128__
+#endif // _MSC_VER
+}
+
+#endif // HASHFUNCS_H

core/containers/list.h

@@ -0,0 +1,699 @@
+#ifndef GLOBALS_LIST_H
+#define GLOBALS_LIST_H
+/*************************************************************************/
+/*  list.h                                                               */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "core/error/error_macros.h"
+#include "core/os/memory.h"
+#include "core/containers/sort_array.h"
+
+/**
+ * Generic Templatized Linked List Implementation.
+ * The implementation differs from the STL one because
+ * a compatible preallocated linked list can be written
+ * using the same API, or features such as erasing an element
+ * from the iterator.
+ */
+
+template <class T, class A = DefaultAllocator>
+class List {
+	struct _Data;
+
+public:
+	class Element {
+	private:
+		friend class List<T, A>;
+
+		T value;
+		Element *next_ptr;
+		Element *prev_ptr;
+		_Data *data;
+
+	public:
+		/**
+		 * Get NEXT Element iterator, for constant lists.
+		 */
+		_FORCE_INLINE_ const Element *next() const {
+			return next_ptr;
+		};
+		/**
+		 * Get NEXT Element iterator,
+		 */
+		_FORCE_INLINE_ Element *next() {
+			return next_ptr;
+		};
+
+		/**
+		 * Get PREV Element iterator, for constant lists.
+		 */
+		_FORCE_INLINE_ const Element *prev() const {
+			return prev_ptr;
+		};
+		/**
+		 * Get PREV Element iterator,
+		 */
+		_FORCE_INLINE_ Element *prev() {
+			return prev_ptr;
+		};
+
+		/**
+		 * * operator, for using as *iterator, when iterators are defined on stack.
+		 */
+		_FORCE_INLINE_ const T &operator*() const {
+			return value;
+		};
+		/**
+		 * operator->, for using as iterator->, when iterators are defined on stack, for constant lists.
+		 */
+		_FORCE_INLINE_ const T *operator->() const {
+			return &value;
+		};
+		/**
+		 * * operator, for using as *iterator, when iterators are defined on stack,
+		 */
+		_FORCE_INLINE_ T &operator*() {
+			return value;
+		};
+		/**
+		 * operator->, for using as iterator->, when iterators are defined on stack, for constant lists.
+		 */
+		_FORCE_INLINE_ T *operator->() {
+			return &value;
+		};
+
+		/**
+		 * get the value stored in this element.
+		 */
+		_FORCE_INLINE_ T &get() {
+			return value;
+		};
+		/**
+		 * get the value stored in this element, for constant lists
+		 */
+		_FORCE_INLINE_ const T &get() const {
+			return value;
+		};
+		/**
+		 * set the value stored in this element.
+		 */
+		_FORCE_INLINE_ void set(const T &p_value) {
+			value = (T &)p_value;
+		};
+
+		void erase() {
+			data->erase(this);
+		}
+
+		_FORCE_INLINE_ Element() {
+			next_ptr = nullptr;
+			prev_ptr = nullptr;
+			data = nullptr;
+		};
+	};
+
+private:
+	struct _Data {
+		Element *first;
+		Element *last;
+		int size_cache;
+
+		bool erase(const Element *p_I) {
+			ERR_FAIL_COND_V(!p_I, false);
+			ERR_FAIL_COND_V(p_I->data != this, false);
+
+			if (first == p_I) {
+				first = p_I->next_ptr;
+			};
+
+			if (last == p_I) {
+				last = p_I->prev_ptr;
+			}
+
+			if (p_I->prev_ptr) {
+				p_I->prev_ptr->next_ptr = p_I->next_ptr;
+			}
+
+			if (p_I->next_ptr) {
+				p_I->next_ptr->prev_ptr = p_I->prev_ptr;
+			}
+
+			memdelete_allocator<Element, A>(const_cast<Element *>(p_I));
+			size_cache--;
+
+			return true;
+		}
+	};
+
+	_Data *_data;
+
+public:
+	/**
+	 * return a const iterator to the beginning of the list.
+	 */
+	_FORCE_INLINE_ const Element *front() const {
+		return _data ? _data->first : nullptr;
+	};
+
+	/**
+	 * return an iterator to the beginning of the list.
+	 */
+	_FORCE_INLINE_ Element *front() {
+		return _data ? _data->first : nullptr;
+	};
+
+	/**
+	 * return a const iterator to the last member of the list.
+	 */
+	_FORCE_INLINE_ const Element *back() const {
+		return _data ? _data->last : nullptr;
+	};
+
+	/**
+	 * return an iterator to the last member of the list.
+	 */
+	_FORCE_INLINE_ Element *back() {
+		return _data ? _data->last : nullptr;
+	};
+
+	/**
+	 * store a new element at the end of the list
+	 */
+	Element *push_back(const T &value) {
+		if (!_data) {
+			_data = memnew_allocator(_Data, A);
+			_data->first = nullptr;
+			_data->last = nullptr;
+			_data->size_cache = 0;
+		}
+
+		Element *n = memnew_allocator(Element, A);
+		n->value = (T &)value;
+
+		n->prev_ptr = _data->last;
+		n->next_ptr = nullptr;
+		n->data = _data;
+
+		if (_data->last) {
+			_data->last->next_ptr = n;
+		}
+
+		_data->last = n;
+
+		if (!_data->first) {
+			_data->first = n;
+		}
+
+		_data->size_cache++;
+
+		return n;
+	};
+
+	void pop_back() {
+		if (_data && _data->last) {
+			erase(_data->last);
+		}
+	}
+
+	/**
+	 * store a new element at the beginning of the list
+	 */
+	Element *push_front(const T &value) {
+		if (!_data) {
+			_data = memnew_allocator(_Data, A);
+			_data->first = nullptr;
+			_data->last = nullptr;
+			_data->size_cache = 0;
+		}
+
+		Element *n = memnew_allocator(Element, A);
+		n->value = (T &)value;
+		n->prev_ptr = nullptr;
+		n->next_ptr = _data->first;
+		n->data = _data;
+
+		if (_data->first) {
+			_data->first->prev_ptr = n;
+		}
+
+		_data->first = n;
+
+		if (!_data->last) {
+			_data->last = n;
+		}
+
+		_data->size_cache++;
+
+		return n;
+	};
+
+	void pop_front() {
+		if (_data && _data->first) {
+			erase(_data->first);
+		}
+	}
+
+	Element *insert_after(Element *p_element, const T &p_value) {
+		CRASH_COND(p_element && (!_data || p_element->data != _data));
+
+		if (!p_element) {
+			return push_back(p_value);
+		}
+
+		Element *n = memnew_allocator(Element, A);
+		n->value = (T &)p_value;
+		n->prev_ptr = p_element;
+		n->next_ptr = p_element->next_ptr;
+		n->data = _data;
+
+		if (!p_element->next_ptr) {
+			_data->last = n;
+		} else {
+			p_element->next_ptr->prev_ptr = n;
+		}
+
+		p_element->next_ptr = n;
+
+		_data->size_cache++;
+
+		return n;
+	}
+
+	Element *insert_before(Element *p_element, const T &p_value) {
+		CRASH_COND(p_element && (!_data || p_element->data != _data));
+
+		if (!p_element) {
+			return push_back(p_value);
+		}
+
+		Element *n = memnew_allocator(Element, A);
+		n->value = (T &)p_value;
+		n->prev_ptr = p_element->prev_ptr;
+		n->next_ptr = p_element;
+		n->data = _data;
+
+		if (!p_element->prev_ptr) {
+			_data->first = n;
+		} else {
+			p_element->prev_ptr->next_ptr = n;
+		}
+
+		p_element->prev_ptr = n;
+
+		_data->size_cache++;
+
+		return n;
+	}
+
+	/**
+	 * find an element in the list,
+	 */
+	template <class T_v>
+	Element *find(const T_v &p_val) {
+		Element *it = front();
+		while (it) {
+			if (it->value == p_val) {
+				return it;
+			}
+			it = it->next();
+		};
+
+		return nullptr;
+	};
+
+	/**
+	 * erase an element in the list, by iterator pointing to it. Return true if it was found/erased.
+	 */
+	bool erase(const Element *p_I) {
+		if (_data) {
+			bool ret = _data->erase(p_I);
+
+			if (_data->size_cache == 0) {
+				memdelete_allocator<_Data, A>(_data);
+				_data = nullptr;
+			}
+
+			return ret;
+		}
+
+		return false;
+	};
+
+	/**
+	 * erase the first element in the list, that contains value
+	 */
+	bool erase(const T &value) {
+		Element *I = find(value);
+		return erase(I);
+	};
+
+	/**
+	 * return whether the list is empty
+	 */
+	_FORCE_INLINE_ bool empty() const {
+		return (!_data || !_data->size_cache);
+	}
+
+	/**
+	 * clear the list
+	 */
+	void clear() {
+		while (front()) {
+			erase(front());
+		};
+	};
+
+	_FORCE_INLINE_ int size() const {
+		return _data ? _data->size_cache : 0;
+	}
+
+	void swap(Element *p_A, Element *p_B) {
+		ERR_FAIL_COND(!p_A || !p_B);
+		ERR_FAIL_COND(p_A->data != _data);
+		ERR_FAIL_COND(p_B->data != _data);
+
+		if (p_A == p_B) {
+			return;
+		}
+		Element *A_prev = p_A->prev_ptr;
+		Element *A_next = p_A->next_ptr;
+		Element *B_prev = p_B->prev_ptr;
+		Element *B_next = p_B->next_ptr;
+
+		if (A_prev) {
+			A_prev->next_ptr = p_B;
+		} else {
+			_data->first = p_B;
+		}
+		if (B_prev) {
+			B_prev->next_ptr = p_A;
+		} else {
+			_data->first = p_A;
+		}
+		if (A_next) {
+			A_next->prev_ptr = p_B;
+		} else {
+			_data->last = p_B;
+		}
+		if (B_next) {
+			B_next->prev_ptr = p_A;
+		} else {
+			_data->last = p_A;
+		}
+		p_A->prev_ptr = A_next == p_B ? p_B : B_prev;
+		p_A->next_ptr = B_next == p_A ? p_B : B_next;
+		p_B->prev_ptr = B_next == p_A ? p_A : A_prev;
+		p_B->next_ptr = A_next == p_B ? p_A : A_next;
+	}
+	/**
+	 * copy the list
+	 */
+	void operator=(const List &p_list) {
+		clear();
+		const Element *it = p_list.front();
+		while (it) {
+			push_back(it->get());
+			it = it->next();
+		}
+	}
+
+	T &operator[](int p_index) {
+		CRASH_BAD_INDEX(p_index, size());
+
+		Element *I = front();
+		int c = 0;
+		while (I) {
+			if (c == p_index) {
+				return I->get();
+			}
+			I = I->next();
+			c++;
+		}
+
+		CRASH_NOW(); // bug!!
+	}
+
+	const T &operator[](int p_index) const {
+		CRASH_BAD_INDEX(p_index, size());
+
+		const Element *I = front();
+		int c = 0;
+		while (I) {
+			if (c == p_index) {
+				return I->get();
+			}
+			I = I->next();
+			c++;
+		}
+
+		CRASH_NOW(); // bug!!
+	}
+
+	void move_to_back(Element *p_I) {
+		ERR_FAIL_COND(p_I->data != _data);
+		if (!p_I->next_ptr) {
+			return;
+		}
+
+		if (_data->first == p_I) {
+			_data->first = p_I->next_ptr;
+		};
+
+		if (_data->last == p_I) {
+			_data->last = p_I->prev_ptr;
+		}
+
+		if (p_I->prev_ptr) {
+			p_I->prev_ptr->next_ptr = p_I->next_ptr;
+		}
+
+		p_I->next_ptr->prev_ptr = p_I->prev_ptr;
+
+		_data->last->next_ptr = p_I;
+		p_I->prev_ptr = _data->last;
+		p_I->next_ptr = nullptr;
+		_data->last = p_I;
+	}
+
+	void invert() {
+		int s = size() / 2;
+		Element *F = front();
+		Element *B = back();
+		for (int i = 0; i < s; i++) {
+			SWAP(F->value, B->value);
+			F = F->next();
+			B = B->prev();
+		}
+	}
+
+	void move_to_front(Element *p_I) {
+		ERR_FAIL_COND(p_I->data != _data);
+		if (!p_I->prev_ptr) {
+			return;
+		}
+
+		if (_data->first == p_I) {
+			_data->first = p_I->next_ptr;
+		};
+
+		if (_data->last == p_I) {
+			_data->last = p_I->prev_ptr;
+		}
+
+		p_I->prev_ptr->next_ptr = p_I->next_ptr;
+
+		if (p_I->next_ptr) {
+			p_I->next_ptr->prev_ptr = p_I->prev_ptr;
+		}
+
+		_data->first->prev_ptr = p_I;
+		p_I->next_ptr = _data->first;
+		p_I->prev_ptr = nullptr;
+		_data->first = p_I;
+	}
+
+	void move_before(Element *value, Element *where) {
+		if (value->prev_ptr) {
+			value->prev_ptr->next_ptr = value->next_ptr;
+		} else {
+			_data->first = value->next_ptr;
+		}
+		if (value->next_ptr) {
+			value->next_ptr->prev_ptr = value->prev_ptr;
+		} else {
+			_data->last = value->prev_ptr;
+		}
+
+		value->next_ptr = where;
+		if (!where) {
+			value->prev_ptr = _data->last;
+			_data->last = value;
+			return;
+		};
+
+		value->prev_ptr = where->prev_ptr;
+
+		if (where->prev_ptr) {
+			where->prev_ptr->next_ptr = value;
+		} else {
+			_data->first = value;
+		};
+
+		where->prev_ptr = value;
+	};
+
+	/**
+	 * simple insertion sort
+	 */
+
+	void sort() {
+		sort_custom<Comparator<T>>();
+	}
+
+	template <class C>
+	void sort_custom_inplace() {
+		if (size() < 2) {
+			return;
+		}
+
+		Element *from = front();
+		Element *current = from;
+		Element *to = from;
+
+		while (current) {
+			Element *next = current->next_ptr;
+
+			if (from != current) {
+				current->prev_ptr = NULL;
+				current->next_ptr = from;
+
+				Element *find = from;
+				C less;
+				while (find && less(find->value, current->value)) {
+					current->prev_ptr = find;
+					current->next_ptr = find->next_ptr;
+					find = find->next_ptr;
+				}
+
+				if (current->prev_ptr) {
+					current->prev_ptr->next_ptr = current;
+				} else {
+					from = current;
+				}
+
+				if (current->next_ptr) {
+					current->next_ptr->prev_ptr = current;
+				} else {
+					to = current;
+				}
+			} else {
+				current->prev_ptr = NULL;
+				current->next_ptr = NULL;
+			}
+
+			current = next;
+		}
+		_data->first = from;
+		_data->last = to;
+	}
+
+	template <class C>
+	struct AuxiliaryComparator {
+		C compare;
+		_FORCE_INLINE_ bool operator()(const Element *a, const Element *b) const {
+			return compare(a->value, b->value);
+		}
+	};
+
+	template <class C>
+	void sort_custom() {
+		//this version uses auxiliary memory for speed.
+		//if you don't want to use auxiliary memory, use the in_place version
+
+		int s = size();
+		if (s < 2) {
+			return;
+		}
+
+		Element **aux_buffer = memnew_arr(Element *, s);
+
+		int idx = 0;
+		for (Element *E = front(); E; E = E->next_ptr) {
+			aux_buffer[idx] = E;
+			idx++;
+		}
+
+		SortArray<Element *, AuxiliaryComparator<C>> sort;
+		sort.sort(aux_buffer, s);
+
+		_data->first = aux_buffer[0];
+		aux_buffer[0]->prev_ptr = nullptr;
+		aux_buffer[0]->next_ptr = aux_buffer[1];
+
+		_data->last = aux_buffer[s - 1];
+		aux_buffer[s - 1]->prev_ptr = aux_buffer[s - 2];
+		aux_buffer[s - 1]->next_ptr = nullptr;
+
+		for (int i = 1; i < s - 1; i++) {
+			aux_buffer[i]->prev_ptr = aux_buffer[i - 1];
+			aux_buffer[i]->next_ptr = aux_buffer[i + 1];
+		}
+
+		memdelete_arr(aux_buffer);
+	}
+
+	const void *id() const {
+		return (void *)_data;
+	}
+
+	/**
+	 * copy constructor for the list
+	 */
+	List(const List &p_list) {
+		_data = nullptr;
+		const Element *it = p_list.front();
+		while (it) {
+			push_back(it->get());
+			it = it->next();
+		}
+	}
+
+	List() {
+		_data = nullptr;
+	};
+	~List() {
+		clear();
+		if (_data) {
+			ERR_FAIL_COND(_data->size_cache);
+			memdelete_allocator<_Data, A>(_data);
+		}
+	};
+};
+
+#endif

core/containers/local_vector.h

@@ -0,0 +1,321 @@
+#ifndef LOCAL_VECTOR_H
+#define LOCAL_VECTOR_H
+/*************************************************************************/
+/*  local_vector.h                                                       */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "core/containers/pool_vector.h"
+#include "core/containers/sort_array.h"
+#include "core/containers/vector.h"
+#include "core/error/error_macros.h"
+#include "core/os/memory.h"
+
+template <class T, class U = uint32_t, bool force_trivial = false>
+class LocalVector {
+protected:
+	U count = 0;
+	U capacity = 0;
+	T *data = nullptr;
+
+public:
+	T *ptr() {
+		return data;
+	}
+
+	const T *ptr() const {
+		return data;
+	}
+
+	_FORCE_INLINE_ void push_back(T p_elem) {
+		if (unlikely(count == capacity)) {
+			if (capacity == 0) {
+				capacity = 1;
+			} else {
+				capacity <<= 1;
+			}
+			data = (T *)memrealloc(data, capacity * sizeof(T));
+			CRASH_COND_MSG(!data, "Out of memory");
+		}
+
+		if (!HAS_TRIVIAL_CONSTRUCTOR(T) && !force_trivial) {
+			memnew_placement(&data[count++], T(p_elem));
+		} else {
+			data[count++] = p_elem;
+		}
+	}
+
+	void remove(U p_index) {
+		ERR_FAIL_UNSIGNED_INDEX(p_index, count);
+		count--;
+		for (U i = p_index; i < count; i++) {
+			data[i] = data[i + 1];
+		}
+		if (!HAS_TRIVIAL_DESTRUCTOR(T) && !force_trivial) {
+			data[count].~T();
+		}
+	}
+
+	// Removes the item copying the last value into the position of the one to
+	// remove. It's generally faster than `remove`.
+	void remove_unordered(U p_index) {
+		ERR_FAIL_INDEX(p_index, count);
+		count--;
+		if (count > p_index) {
+			data[p_index] = data[count];
+		}
+		if (!HAS_TRIVIAL_DESTRUCTOR(T) && !force_trivial) {
+			data[count].~T();
+		}
+	}
+
+	_FORCE_INLINE_ bool erase(const T &p_val) {
+		int64_t idx = find(p_val);
+		if (idx >= 0) {
+			remove(idx);
+			return true;
+		}
+		return false;
+	}
+
+	U erase_multiple_unordered(const T &p_val) {
+		U from = 0;
+		U count = 0;
+		while (true) {
+			int64_t idx = find(p_val, from);
+
+			if (idx == -1) {
+				break;
+			}
+			remove_unordered(idx);
+			from = idx;
+			count++;
+		}
+		return count;
+	}
+
+	void invert() {
+		for (U i = 0; i < count / 2; i++) {
+			SWAP(data[i], data[count - i - 1]);
+		}
+	}
+
+	_FORCE_INLINE_ void clear() { resize(0); }
+	_FORCE_INLINE_ void reset() {
+		clear();
+		if (data) {
+			memfree(data);
+			data = nullptr;
+			capacity = 0;
+		}
+	}
+	_FORCE_INLINE_ bool empty() const { return count == 0; }
+	_FORCE_INLINE_ U get_capacity() const { return capacity; }
+	_FORCE_INLINE_ void reserve(U p_size, bool p_allow_shrink = false) {
+		p_size = nearest_power_of_2_templated(p_size);
+		if (!p_allow_shrink ? p_size > capacity : ((p_size >= count) && (p_size != capacity))) {
+			capacity = p_size;
+			data = (T *)memrealloc(data, capacity * sizeof(T));
+			CRASH_COND_MSG(!data, "Out of memory");
+		}
+	}
+
+	_FORCE_INLINE_ U size() const { return count; }
+	void resize(U p_size) {
+		if (p_size < count) {
+			if (!HAS_TRIVIAL_DESTRUCTOR(T) && !force_trivial) {
+				for (U i = p_size; i < count; i++) {
+					data[i].~T();
+				}
+			}
+			count = p_size;
+		} else if (p_size > count) {
+			if (unlikely(p_size > capacity)) {
+				if (capacity == 0) {
+					capacity = 1;
+				}
+				while (capacity < p_size) {
+					capacity <<= 1;
+				}
+				data = (T *)memrealloc(data, capacity * sizeof(T));
+				CRASH_COND_MSG(!data, "Out of memory");
+			}
+			if (!HAS_TRIVIAL_CONSTRUCTOR(T) && !force_trivial) {
+				for (U i = count; i < p_size; i++) {
+					memnew_placement(&data[i], T);
+				}
+			}
+			count = p_size;
+		}
+	}
+	_FORCE_INLINE_ const T &operator[](U p_index) const {
+		CRASH_BAD_UNSIGNED_INDEX(p_index, count);
+		return data[p_index];
+	}
+	_FORCE_INLINE_ T &operator[](U p_index) {
+		CRASH_BAD_UNSIGNED_INDEX(p_index, count);
+		return data[p_index];
+	}
+
+	void fill(T p_val) {
+		for (U i = 0; i < count; i++) {
+			data[i] = p_val;
+		}
+	}
+
+	void insert(U p_pos, T p_val) {
+		ERR_FAIL_UNSIGNED_INDEX(p_pos, count + 1);
+		if (p_pos == count) {
+			push_back(p_val);
+		} else {
+			resize(count + 1);
+			for (U i = count - 1; i > p_pos; i--) {
+				data[i] = data[i - 1];
+			}
+			data[p_pos] = p_val;
+		}
+	}
+
+	int64_t find(const T &p_val, U p_from = 0) const {
+		for (U i = p_from; i < count; i++) {
+			if (data[i] == p_val) {
+				return int64_t(i);
+			}
+		}
+		return -1;
+	}
+
+	template <class C>
+	void sort_custom() {
+		U len = count;
+		if (len == 0) {
+			return;
+		}
+
+		SortArray<T, C> sorter;
+		sorter.sort(data, len);
+	}
+
+	void sort() {
+		sort_custom<_DefaultComparator<T>>();
+	}
+
+	void ordered_insert(T p_val) {
+		U i;
+		for (i = 0; i < count; i++) {
+			if (p_val < data[i]) {
+				break;
+			}
+		}
+		insert(i, p_val);
+	}
+
+	operator Vector<T>() const {
+		Vector<T> ret;
+		ret.resize(size());
+		T *w = ret.ptrw();
+		memcpy(w, data, sizeof(T) * count);
+		return ret;
+	}
+
+	operator PoolVector<T>() const {
+		PoolVector<T> pl;
+		if (size()) {
+			pl.resize(size());
+			typename PoolVector<T>::Write w = pl.write();
+			T *dest = w.ptr();
+			memcpy(dest, data, sizeof(T) * count);
+		}
+		return pl;
+	}
+
+	Vector<uint8_t> to_byte_array() const { //useful to pass stuff to gpu or variant
+		Vector<uint8_t> ret;
+		ret.resize(count * sizeof(T));
+		uint8_t *w = ret.ptrw();
+		memcpy(w, data, sizeof(T) * count);
+		return ret;
+	}
+
+	_FORCE_INLINE_ LocalVector() {}
+	_FORCE_INLINE_ LocalVector(const LocalVector &p_from) {
+		resize(p_from.size());
+		for (U i = 0; i < p_from.count; i++) {
+			data[i] = p_from.data[i];
+		}
+	}
+	LocalVector(const Vector<T> &p_from) {
+		resize(p_from.size());
+		for (U i = 0; i < count; i++) {
+			data[i] = p_from[i];
+		}
+	}
+	LocalVector(const PoolVector<T> &p_from) {
+		resize(p_from.size());
+		typename PoolVector<T>::Read r = p_from.read();
+		for (U i = 0; i < count; i++) {
+			data[i] = r[i];
+		}
+	}
+
+	inline LocalVector &operator=(const LocalVector &p_from) {
+		resize(p_from.size());
+		for (U i = 0; i < p_from.count; i++) {
+			data[i] = p_from.data[i];
+		}
+		return *this;
+	}
+	inline LocalVector &operator=(const Vector<T> &p_from) {
+		resize(p_from.size());
+		for (U i = 0; i < count; i++) {
+			data[i] = p_from[i];
+		}
+		return *this;
+	}
+	inline LocalVector &operator=(const PoolVector<T> &p_from) {
+		resize(p_from.size());
+		typename PoolVector<T>::Read r = p_from.read();
+		for (U i = 0; i < count; i++) {
+			data[i] = r[i];
+		}
+		return *this;
+	}
+
+	_FORCE_INLINE_ ~LocalVector() {
+		if (data) {
+			reset();
+		}
+	}
+};
+
+// Integer default version
+template <class T, class I = int32_t, bool force_trivial = false>
+class LocalVectori : public LocalVector<T, I, force_trivial> {
+};
+
+#endif // LOCAL_VECTOR_H

core/containers/lru.h

@@ -0,0 +1,127 @@
+/**************************************************************************/
+/*  lru.h                                                                 */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef LRU_H
+#define LRU_H
+
+#include "core/math/math_funcs.h"
+#include "hash_map.h"
+#include "list.h"
+
+template <class TKey, class TData, class Hasher = HashMapHasherDefault, class Comparator = HashMapComparatorDefault<TKey>>
+class LRUCache {
+private:
+	struct Pair {
+		TKey key;
+		TData data;
+
+		Pair() {}
+		Pair(const TKey &p_key, const TData &p_data) :
+				key(p_key),
+				data(p_data) {
+		}
+	};
+
+	typedef typename List<Pair>::Element *Element;
+
+	List<Pair> _list;
+	HashMap<TKey, Element, Hasher, Comparator> _map;
+	size_t capacity;
+
+public:
+	const TData *insert(const TKey &p_key, const TData &p_value) {
+		Element *e = _map.getptr(p_key);
+		Element n = _list.push_front(Pair(p_key, p_value));
+
+		if (e) {
+			_list.erase(*e);
+			_map.erase(p_key);
+		}
+		_map[p_key] = _list.front();
+
+		while (_map.size() > capacity) {
+			Element d = _list.back();
+			_map.erase(d->get().key);
+			_list.pop_back();
+		}
+
+		return &n->get().data;
+	}
+
+	void clear() {
+		_map.clear();
+		_list.clear();
+	}
+
+	bool has(const TKey &p_key) const {
+		return _map.getptr(p_key);
+	}
+
+	const TData &get(const TKey &p_key) {
+		Element *e = _map.getptr(p_key);
+		CRASH_COND(!e);
+		_list.move_to_front(*e);
+		return (*e)->get().data;
+	};
+
+	const TData *getptr(const TKey &p_key) {
+		Element *e = _map.getptr(p_key);
+		if (!e) {
+			return nullptr;
+		} else {
+			_list.move_to_front(*e);
+			return &(*e)->get().data;
+		}
+	}
+
+	_FORCE_INLINE_ size_t get_capacity() const { return capacity; }
+	_FORCE_INLINE_ size_t get_size() const { return _map.size(); }
+
+	void set_capacity(size_t p_capacity) {
+		if (capacity > 0) {
+			capacity = p_capacity;
+			while (_map.size() > capacity) {
+				Element d = _list.back();
+				_map.erase(d->get().key);
+				_list.pop_back();
+			}
+		}
+	}
+
+	LRUCache() {
+		capacity = 64;
+	}
+
+	LRUCache(int p_capacity) {
+		capacity = p_capacity;
+	}
+};
+
+#endif // LRU_H

core/containers/oa_hash_map.h

@@ -0,0 +1,380 @@
+#ifndef OA_HASH_MAP_H
+#define OA_HASH_MAP_H
+/*************************************************************************/
+/*  oa_hash_map.h                                                        */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "core/containers/hashfuncs.h"
+#include "core/math/math_funcs.h"
+#include "core/os/memory.h"
+
+/**
+ * A HashMap implementation that uses open addressing with Robin Hood hashing.
+ * Robin Hood hashing swaps out entries that have a smaller probing distance
+ * than the to-be-inserted entry, that evens out the average probing distance
+ * and enables faster lookups. Backward shift deletion is employed to further
+ * improve the performance and to avoid infinite loops in rare cases.
+ *
+ * The entries are stored inplace, so huge keys or values might fill cache lines
+ * a lot faster.
+ *
+ * Only used keys and values are constructed. For free positions there's space
+ * in the arrays for each, but that memory is kept uninitialized.
+ */
+template <class TKey, class TValue,
+		class Hasher = HashMapHasherDefault,
+		class Comparator = HashMapComparatorDefault<TKey>>
+class OAHashMap {
+private:
+	TValue *values;
+	TKey *keys;
+	uint32_t *hashes;
+
+	uint32_t capacity;
+
+	uint32_t num_elements;
+
+	static const uint32_t EMPTY_HASH = 0;
+
+	_FORCE_INLINE_ uint32_t _hash(const TKey &p_key) const {
+		uint32_t hash = Hasher::hash(p_key);
+
+		if (hash == EMPTY_HASH) {
+			hash = EMPTY_HASH + 1;
+		}
+
+		return hash;
+	}
+
+	_FORCE_INLINE_ uint32_t _get_probe_length(uint32_t p_pos, uint32_t p_hash) const {
+		uint32_t original_pos = p_hash % capacity;
+		return (p_pos - original_pos + capacity) % capacity;
+	}
+
+	_FORCE_INLINE_ void _construct(uint32_t p_pos, uint32_t p_hash, const TKey &p_key, const TValue &p_value) {
+		memnew_placement(&keys[p_pos], TKey(p_key));
+		memnew_placement(&values[p_pos], TValue(p_value));
+		hashes[p_pos] = p_hash;
+
+		num_elements++;
+	}
+
+	bool _lookup_pos(const TKey &p_key, uint32_t &r_pos) const {
+		uint32_t hash = _hash(p_key);
+		uint32_t pos = hash % capacity;
+		uint32_t distance = 0;
+
+		while (true) {
+			if (hashes[pos] == EMPTY_HASH) {
+				return false;
+			}
+
+			if (distance > _get_probe_length(pos, hashes[pos])) {
+				return false;
+			}
+
+			if (hashes[pos] == hash && Comparator::compare(keys[pos], p_key)) {
+				r_pos = pos;
+				return true;
+			}
+
+			pos = (pos + 1) % capacity;
+			distance++;
+		}
+	}
+
+	void _insert_with_hash(uint32_t p_hash, const TKey &p_key, const TValue &p_value) {
+		uint32_t hash = p_hash;
+		uint32_t distance = 0;
+		uint32_t pos = hash % capacity;
+
+		TKey key = p_key;
+		TValue value = p_value;
+
+		while (true) {
+			if (hashes[pos] == EMPTY_HASH) {
+				_construct(pos, hash, key, value);
+
+				return;
+			}
+
+			// not an empty slot, let's check the probing length of the existing one
+			uint32_t existing_probe_len = _get_probe_length(pos, hashes[pos]);
+			if (existing_probe_len < distance) {
+				SWAP(hash, hashes[pos]);
+				SWAP(key, keys[pos]);
+				SWAP(value, values[pos]);
+				distance = existing_probe_len;
+			}
+
+			pos = (pos + 1) % capacity;
+			distance++;
+		}
+	}
+
+	void _resize_and_rehash(uint32_t p_new_capacity) {
+		uint32_t old_capacity = capacity;
+		capacity = p_new_capacity;
+
+		TKey *old_keys = keys;
+		TValue *old_values = values;
+		uint32_t *old_hashes = hashes;
+
+		num_elements = 0;
+		keys = static_cast<TKey *>(Memory::alloc_static(sizeof(TKey) * capacity));
+		values = static_cast<TValue *>(Memory::alloc_static(sizeof(TValue) * capacity));
+		hashes = static_cast<uint32_t *>(Memory::alloc_static(sizeof(uint32_t) * capacity));
+
+		for (uint32_t i = 0; i < capacity; i++) {
+			hashes[i] = 0;
+		}
+
+		for (uint32_t i = 0; i < old_capacity; i++) {
+			if (old_hashes[i] == EMPTY_HASH) {
+				continue;
+			}
+
+			_insert_with_hash(old_hashes[i], old_keys[i], old_values[i]);
+
+			old_keys[i].~TKey();
+			old_values[i].~TValue();
+		}
+
+		Memory::free_static(old_keys);
+		Memory::free_static(old_values);
+		Memory::free_static(old_hashes);
+	}
+
+	void _resize_and_rehash() {
+		_resize_and_rehash(capacity * 2);
+	}
+
+public:
+	_FORCE_INLINE_ uint32_t get_capacity() const { return capacity; }
+	_FORCE_INLINE_ uint32_t get_num_elements() const { return num_elements; }
+
+	bool empty() const {
+		return num_elements == 0;
+	}
+
+	void clear() {
+		for (uint32_t i = 0; i < capacity; i++) {
+			if (hashes[i] == EMPTY_HASH) {
+				continue;
+			}
+
+			hashes[i] = EMPTY_HASH;
+			values[i].~TValue();
+			keys[i].~TKey();
+		}
+
+		num_elements = 0;
+	}
+
+	void insert(const TKey &p_key, const TValue &p_value) {
+		if (num_elements + 1 > 0.9 * capacity) {
+			_resize_and_rehash();
+		}
+
+		uint32_t hash = _hash(p_key);
+
+		_insert_with_hash(hash, p_key, p_value);
+	}
+
+	void set(const TKey &p_key, const TValue &p_data) {
+		uint32_t pos = 0;
+		bool exists = _lookup_pos(p_key, pos);
+
+		if (exists) {
+			values[pos] = p_data;
+		} else {
+			insert(p_key, p_data);
+		}
+	}
+
+	/**
+	 * returns true if the value was found, false otherwise.
+	 *
+	 * if r_data is not NULL then the value will be written to the object
+	 * it points to.
+	 */
+	bool lookup(const TKey &p_key, TValue &r_data) const {
+		uint32_t pos = 0;
+		bool exists = _lookup_pos(p_key, pos);
+
+		if (exists) {
+			r_data = values[pos];
+			return true;
+		}
+
+		return false;
+	}
+
+	const TValue *lookup_ptr_const(const TKey &p_key) const {
+		uint32_t pos = 0;
+		bool exists = _lookup_pos(p_key, pos);
+
+		if (exists) {
+			return &values[pos];
+		}
+
+		return nullptr;
+	}
+
+	TValue *lookup_ptr(const TKey &p_key) const {
+		uint32_t pos = 0;
+		bool exists = _lookup_pos(p_key, pos);
+
+		if (exists) {
+			return &values[pos];
+		}
+
+		return nullptr;
+	}
+
+	_FORCE_INLINE_ bool has(const TKey &p_key) const {
+		uint32_t _pos = 0;
+		return _lookup_pos(p_key, _pos);
+	}
+
+	void remove(const TKey &p_key) {
+		uint32_t pos = 0;
+		bool exists = _lookup_pos(p_key, pos);
+
+		if (!exists) {
+			return;
+		}
+
+		uint32_t next_pos = (pos + 1) % capacity;
+		while (hashes[next_pos] != EMPTY_HASH &&
+				_get_probe_length(next_pos, hashes[next_pos]) != 0) {
+			SWAP(hashes[next_pos], hashes[pos]);
+			SWAP(keys[next_pos], keys[pos]);
+			SWAP(values[next_pos], values[pos]);
+			pos = next_pos;
+			next_pos = (pos + 1) % capacity;
+		}
+
+		hashes[pos] = EMPTY_HASH;
+		values[pos].~TValue();
+		keys[pos].~TKey();
+
+		num_elements--;
+	}
+
+	/**
+	 * reserves space for a number of elements, useful to avoid many resizes and rehashes
+	 *  if adding a known (possibly large) number of elements at once, must be larger than old
+	 *  capacity.
+	 **/
+	void reserve(uint32_t p_new_capacity) {
+		ERR_FAIL_COND(p_new_capacity < capacity);
+		_resize_and_rehash(p_new_capacity);
+	}
+
+	struct Iterator {
+		bool valid;
+
+		const TKey *key;
+		TValue *value;
+
+	private:
+		uint32_t pos;
+		friend class OAHashMap;
+	};
+
+	Iterator iter() const {
+		Iterator it;
+
+		it.valid = true;
+		it.pos = 0;
+
+		return next_iter(it);
+	}
+
+	Iterator next_iter(const Iterator &p_iter) const {
+		if (!p_iter.valid) {
+			return p_iter;
+		}
+
+		Iterator it;
+		it.valid = false;
+		it.pos = p_iter.pos;
+		it.key = nullptr;
+		it.value = nullptr;
+
+		for (uint32_t i = it.pos; i < capacity; i++) {
+			it.pos = i + 1;
+
+			if (hashes[i] == EMPTY_HASH) {
+				continue;
+			}
+
+			it.valid = true;
+			it.key = &keys[i];
+			it.value = &values[i];
+			return it;
+		}
+
+		return it;
+	}
+
+	OAHashMap(const OAHashMap &) = delete; // Delete the copy constructor so we don't get unexpected copies and dangling pointers.
+	OAHashMap &operator=(const OAHashMap &) = delete; // Same for assignment operator.
+
+	OAHashMap(uint32_t p_initial_capacity = 64) {
+		capacity = p_initial_capacity;
+		num_elements = 0;
+
+		keys = static_cast<TKey *>(Memory::alloc_static(sizeof(TKey) * capacity));
+		values = static_cast<TValue *>(Memory::alloc_static(sizeof(TValue) * capacity));
+		hashes = static_cast<uint32_t *>(Memory::alloc_static(sizeof(uint32_t) * capacity));
+
+		for (uint32_t i = 0; i < p_initial_capacity; i++) {
+			hashes[i] = EMPTY_HASH;
+		}
+	}
+
+	~OAHashMap() {
+		for (uint32_t i = 0; i < capacity; i++) {
+			if (hashes[i] == EMPTY_HASH) {
+				continue;
+			}
+
+			values[i].~TValue();
+			keys[i].~TKey();
+		}
+
+		Memory::free_static(keys);
+		Memory::free_static(values);
+		Memory::free_static(hashes);
+	}
+};
+
+#endif

core/containers/og_hash_map.h

@@ -0,0 +1,604 @@
+#ifndef GHASH_MAP_H
+#define GHASH_MAP_H
+
+/*************************************************************************/
+/*  hash_map.h                                                           */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "core/containers/hashfuncs.h"
+#include "core/containers/list.h"
+#include "core/error/error_macros.h"
+#include "core/math/math_funcs.h"
+#include "core/os/memory.h"
+#include "core/string/ustring.h"
+
+/**
+ * @class OGHashMap
+ * @author Juan Linietsky <reduzio@gmail.com>
+ *
+ * Implementation of a standard Hashing HashMap, for quick lookups of Data associated with a Key.
+ * The implementation provides hashers for the default types, if you need a special kind of hasher, provide
+ * your own.
+ * @param TKey  Key, search is based on it, needs to be hasheable. It is unique in this container.
+ * @param TData Data, data associated with the key
+ * @param Hasher Hasher object, needs to provide a valid static hash function for TKey
+ * @param Comparator comparator object, needs to be able to safely compare two TKey values. It needs to ensure that x == x for any items inserted in the map. Bear in mind that nan != nan when implementing an equality check.
+ * @param MIN_HASH_TABLE_POWER Miminum size of the hash table, as a power of two. You rarely need to change this parameter.
+ * @param RELATIONSHIP Relationship at which the hash table is resized. if amount of elements is RELATIONSHIP
+ * times bigger than the hash table, table is resized to solve this condition. if RELATIONSHIP is zero, table is always MIN_HASH_TABLE_POWER.
+ *
+ */
+
+template <class TKey, class TData, class Hasher = HashMapHasherDefault, class Comparator = HashMapComparatorDefault<TKey>, uint8_t MIN_HASH_TABLE_POWER = 3, uint8_t RELATIONSHIP = 8>
+class OGHashMap {
+public:
+	struct Pair {
+		TKey key;
+		TData data;
+
+		Pair(const TKey &p_key) :
+				key(p_key),
+				data() {}
+		Pair(const TKey &p_key, const TData &p_data) :
+				key(p_key),
+				data(p_data) {
+		}
+	};
+
+	struct Element {
+	private:
+		friend class OGHashMap;
+
+		uint32_t hash;
+		Element *next;
+		Element() { next = nullptr; }
+		Pair pair;
+
+	public:
+		const TKey &key() const {
+			return pair.key;
+		}
+
+		TData &value() {
+			return pair.data;
+		}
+
+		const TData &value() const {
+			return pair.data;
+		}
+
+		TData &get() {
+			return pair.data;
+		};
+		const TData &get() const {
+			return pair.data;
+		};
+
+		Element(const TKey &p_key) :
+				pair(p_key) {}
+		Element(const Element &p_other) :
+				hash(p_other.hash),
+				pair(p_other.pair.key, p_other.pair.data) {}
+	};
+
+private:
+	Element **hash_table;
+	uint8_t hash_table_power;
+	uint32_t elements;
+
+	void make_hash_table() {
+		ERR_FAIL_COND(hash_table);
+
+		hash_table = memnew_arr(Element *, (1 << MIN_HASH_TABLE_POWER));
+
+		hash_table_power = MIN_HASH_TABLE_POWER;
+		elements = 0;
+		for (int i = 0; i < (1 << MIN_HASH_TABLE_POWER); i++) {
+			hash_table[i] = nullptr;
+		}
+	}
+
+	void erase_hash_table() {
+		ERR_FAIL_COND_MSG(elements, "Cannot erase hash table if there are still elements inside.");
+
+		memdelete_arr(hash_table);
+		hash_table = nullptr;
+		hash_table_power = 0;
+		elements = 0;
+	}
+
+	void check_hash_table() {
+		int new_hash_table_power = -1;
+
+		if ((int)elements > ((1 << hash_table_power) * RELATIONSHIP)) {
+			/* rehash up */
+			new_hash_table_power = hash_table_power + 1;
+
+			while ((int)elements > ((1 << new_hash_table_power) * RELATIONSHIP)) {
+				new_hash_table_power++;
+			}
+
+		} else if ((hash_table_power > (int)MIN_HASH_TABLE_POWER) && ((int)elements < ((1 << (hash_table_power - 1)) * RELATIONSHIP))) {
+			/* rehash down */
+			new_hash_table_power = hash_table_power - 1;
+
+			while ((int)elements < ((1 << (new_hash_table_power - 1)) * RELATIONSHIP)) {
+				new_hash_table_power--;
+			}
+
+			if (new_hash_table_power < (int)MIN_HASH_TABLE_POWER) {
+				new_hash_table_power = MIN_HASH_TABLE_POWER;
+			}
+		}
+
+		if (new_hash_table_power == -1) {
+			return;
+		}
+
+		Element **new_hash_table = memnew_arr(Element *, ((uint64_t)1 << new_hash_table_power));
+		ERR_FAIL_COND_MSG(!new_hash_table, "Out of memory.");
+
+		for (int i = 0; i < (1 << new_hash_table_power); i++) {
+			new_hash_table[i] = nullptr;
+		}
+
+		if (hash_table) {
+			for (int i = 0; i < (1 << hash_table_power); i++) {
+				while (hash_table[i]) {
+					Element *se = hash_table[i];
+					hash_table[i] = se->next;
+					int new_pos = se->hash & ((1 << new_hash_table_power) - 1);
+					se->next = new_hash_table[new_pos];
+					new_hash_table[new_pos] = se;
+				}
+			}
+
+			memdelete_arr(hash_table);
+		}
+		hash_table = new_hash_table;
+		hash_table_power = new_hash_table_power;
+	}
+
+	/* I want to have only one function.. */
+	_FORCE_INLINE_ const Element *get_element(const TKey &p_key) const {
+		uint32_t hash = Hasher::hash(p_key);
+		uint32_t index = hash & ((1 << hash_table_power) - 1);
+
+		Element *e = hash_table[index];
+
+		while (e) {
+			/* checking hash first avoids comparing key, which may take longer */
+			if (e->hash == hash && Comparator::compare(e->pair.key, p_key)) {
+				/* the pair exists in this hashtable, so just update data */
+				return e;
+			}
+
+			e = e->next;
+		}
+
+		return nullptr;
+	}
+
+	Element *create_element(const TKey &p_key) {
+		/* if element doesn't exist, create it */
+		Element *e = memnew(Element(p_key));
+		ERR_FAIL_COND_V_MSG(!e, nullptr, "Out of memory.");
+		uint32_t hash = Hasher::hash(p_key);
+		uint32_t index = hash & ((1 << hash_table_power) - 1);
+		e->next = hash_table[index];
+		e->hash = hash;
+
+		hash_table[index] = e;
+		elements++;
+
+		return e;
+	}
+
+	void copy_from(const OGHashMap &p_t) {
+		if (&p_t == this) {
+			return; /* much less bother with that */
+		}
+
+		clear();
+
+		if (!p_t.hash_table || p_t.hash_table_power == 0) {
+			return; /* not copying from empty table */
+		}
+
+		hash_table = memnew_arr(Element *, (uint64_t)1 << p_t.hash_table_power);
+		hash_table_power = p_t.hash_table_power;
+		elements = p_t.elements;
+
+		for (int i = 0; i < (1 << p_t.hash_table_power); i++) {
+			hash_table[i] = nullptr;
+
+			const Element *e = p_t.hash_table[i];
+
+			while (e) {
+				Element *le = memnew(Element(*e)); /* local element */
+
+				/* add to list and reassign pointers */
+				le->next = hash_table[i];
+				hash_table[i] = le;
+
+				e = e->next;
+			}
+		}
+	}
+
+public:
+	Element *set(const TKey &p_key, const TData &p_data) {
+		return set(Pair(p_key, p_data));
+	}
+
+	Element *set(const Pair &p_pair) {
+		Element *e = nullptr;
+		if (!hash_table) {
+			make_hash_table(); // if no table, make one
+		} else {
+			e = const_cast<Element *>(get_element(p_pair.key));
+		}
+
+		/* if we made it up to here, the pair doesn't exist, create and assign */
+
+		if (!e) {
+			e = create_element(p_pair.key);
+			if (!e) {
+				return nullptr;
+			}
+			check_hash_table(); // perform mantenience routine
+		}
+
+		e->pair.data = p_pair.data;
+		return e;
+	}
+
+	bool has(const TKey &p_key) const {
+		return getptr(p_key) != nullptr;
+	}
+
+	/**
+	 * Get a key from data, return a const reference.
+	 * WARNING: this doesn't check errors, use either getptr and check NULL, or check
+	 * first with has(key)
+	 */
+
+	const TData &get(const TKey &p_key) const {
+		const TData *res = getptr(p_key);
+		CRASH_COND_MSG(!res, "Map key not found.");
+		return *res;
+	}
+
+	TData &get(const TKey &p_key) {
+		TData *res = getptr(p_key);
+		CRASH_COND_MSG(!res, "Map key not found.");
+		return *res;
+	}
+
+	/**
+	 * Same as get, except it can return NULL when item was not found.
+	 * This is mainly used for speed purposes.
+	 */
+
+	_FORCE_INLINE_ TData *getptr(const TKey &p_key) {
+		if (unlikely(!hash_table)) {
+			return nullptr;
+		}
+
+		Element *e = const_cast<Element *>(get_element(p_key));
+
+		if (e) {
+			return &e->pair.data;
+		}
+
+		return nullptr;
+	}
+
+	_FORCE_INLINE_ const TData *getptr(const TKey &p_key) const {
+		if (unlikely(!hash_table)) {
+			return nullptr;
+		}
+
+		const Element *e = const_cast<Element *>(get_element(p_key));
+
+		if (e) {
+			return &e->pair.data;
+		}
+
+		return nullptr;
+	}
+
+	const Element *find(const TKey &p_key) const {
+		if (unlikely(!hash_table)) {
+			return nullptr;
+		}
+
+		const Element *e = const_cast<Element *>(get_element(p_key));
+
+		return e;
+	}
+
+	Element *find(const TKey &p_key) {
+		if (unlikely(!hash_table)) {
+			return nullptr;
+		}
+
+		Element *e = const_cast<Element *>(get_element(p_key));
+
+		return e;
+	}
+
+	/**
+	 * Same as get, except it can return NULL when item was not found.
+	 * This version is custom, will take a hash and a custom key (that should support operator==()
+	 */
+
+	template <class C>
+	_FORCE_INLINE_ TData *custom_getptr(C p_custom_key, uint32_t p_custom_hash) {
+		if (unlikely(!hash_table)) {
+			return nullptr;
+		}
+
+		uint32_t hash = p_custom_hash;
+		uint32_t index = hash & ((1 << hash_table_power) - 1);
+
+		Element *e = hash_table[index];
+
+		while (e) {
+			/* checking hash first avoids comparing key, which may take longer */
+			if (e->hash == hash && Comparator::compare(e->pair.key, p_custom_key)) {
+				/* the pair exists in this hashtable, so just update data */
+				return &e->pair.data;
+			}
+
+			e = e->next;
+		}
+
+		return nullptr;
+	}
+
+	template <class C>
+	_FORCE_INLINE_ const TData *custom_getptr(C p_custom_key, uint32_t p_custom_hash) const {
+		if (unlikely(!hash_table)) {
+			return NULL;
+		}
+
+		uint32_t hash = p_custom_hash;
+		uint32_t index = hash & ((1 << hash_table_power) - 1);
+
+		const Element *e = hash_table[index];
+
+		while (e) {
+			/* checking hash first avoids comparing key, which may take longer */
+			if (e->hash == hash && Comparator::compare(e->pair.key, p_custom_key)) {
+				/* the pair exists in this hashtable, so just update data */
+				return &e->pair.data;
+			}
+
+			e = e->next;
+		}
+
+		return NULL;
+	}
+
+	/**
+	 * Erase an item, return true if erasing was successful
+	 */
+
+	bool erase(const TKey &p_key) {
+		if (unlikely(!hash_table)) {
+			return false;
+		}
+
+		uint32_t hash = Hasher::hash(p_key);
+		uint32_t index = hash & ((1 << hash_table_power) - 1);
+
+		Element *e = hash_table[index];
+		Element *p = nullptr;
+		while (e) {
+			/* checking hash first avoids comparing key, which may take longer */
+			if (e->hash == hash && Comparator::compare(e->pair.key, p_key)) {
+				if (p) {
+					p->next = e->next;
+				} else {
+					//begin of list
+					hash_table[index] = e->next;
+				}
+
+				memdelete(e);
+				elements--;
+
+				if (elements == 0) {
+					erase_hash_table();
+				} else {
+					check_hash_table();
+				}
+				return true;
+			}
+
+			p = e;
+			e = e->next;
+		}
+
+		return false;
+	}
+
+	inline const TData &operator[](const TKey &p_key) const { //constref
+
+		return get(p_key);
+	}
+	inline TData &operator[](const TKey &p_key) { //assignment
+
+		Element *e = nullptr;
+		if (!hash_table) {
+			make_hash_table(); // if no table, make one
+		} else {
+			e = const_cast<Element *>(get_element(p_key));
+		}
+
+		/* if we made it up to here, the pair doesn't exist, create */
+		if (!e) {
+			e = create_element(p_key);
+			CRASH_COND(!e);
+			check_hash_table(); // perform mantenience routine
+		}
+
+		return e->pair.data;
+	}
+
+	/**
+	 * Get the next key to p_key, and the first key if p_key is null.
+	 * Returns a pointer to the next key if found, NULL otherwise.
+	 * Adding/Removing elements while iterating will, of course, have unexpected results, don't do it.
+	 *
+	 * Example:
+	 *
+	 * 	const TKey *k=NULL;
+	 *
+	 * 	while( (k=table.next(k)) ) {
+	 *
+	 * 		print( *k );
+	 * 	}
+	 *
+	 */
+	const TKey *next(const TKey *p_key) const {
+		if (unlikely(!hash_table)) {
+			return nullptr;
+		}
+
+		if (!p_key) { /* get the first key */
+
+			for (int i = 0; i < (1 << hash_table_power); i++) {
+				if (hash_table[i]) {
+					return &hash_table[i]->pair.key;
+				}
+			}
+
+		} else { /* get the next key */
+
+			const Element *e = get_element(*p_key);
+			ERR_FAIL_COND_V_MSG(!e, nullptr, "Invalid key supplied.");
+			if (e->next) {
+				/* if there is a "next" in the list, return that */
+				return &e->next->pair.key;
+			} else {
+				/* go to next elements */
+				uint32_t index = e->hash & ((1 << hash_table_power) - 1);
+				index++;
+				for (int i = index; i < (1 << hash_table_power); i++) {
+					if (hash_table[i]) {
+						return &hash_table[i]->pair.key;
+					}
+				}
+			}
+
+			/* nothing found, was at end */
+		}
+
+		return nullptr; /* nothing found */
+	}
+
+	inline unsigned int size() const {
+		return elements;
+	}
+
+	inline bool empty() const {
+		return elements == 0;
+	}
+
+	void clear() {
+		/* clean up */
+		if (hash_table) {
+			for (int i = 0; i < (1 << hash_table_power); i++) {
+				while (hash_table[i]) {
+					Element *e = hash_table[i];
+					hash_table[i] = e->next;
+					memdelete(e);
+				}
+			}
+
+			memdelete_arr(hash_table);
+		}
+
+		hash_table = nullptr;
+		hash_table_power = 0;
+		elements = 0;
+	}
+
+	void operator=(const OGHashMap &p_table) {
+		copy_from(p_table);
+	}
+
+	OGHashMap() {
+		hash_table = nullptr;
+		elements = 0;
+		hash_table_power = 0;
+	}
+
+	void get_key_value_ptr_array(const Pair **p_pairs) const {
+		if (unlikely(!hash_table)) {
+			return;
+		}
+		for (int i = 0; i < (1 << hash_table_power); i++) {
+			Element *e = hash_table[i];
+			while (e) {
+				*p_pairs = &e->pair;
+				p_pairs++;
+				e = e->next;
+			}
+		}
+	}
+
+	void get_key_list(List<TKey> *p_keys) const {
+		if (unlikely(!hash_table)) {
+			return;
+		}
+		for (int i = 0; i < (1 << hash_table_power); i++) {
+			Element *e = hash_table[i];
+			while (e) {
+				p_keys->push_back(e->pair.key);
+				e = e->next;
+			}
+		}
+	}
+
+	OGHashMap(const OGHashMap &p_table) {
+		hash_table = nullptr;
+		elements = 0;
+		hash_table_power = 0;
+
+		copy_from(p_table);
+	}
+
+	~OGHashMap() {
+		clear();
+	}
+};
+
+#endif

core/containers/ordered_hash_map.h

@@ -0,0 +1,309 @@
+#ifndef ORDERED_HASH_MAP_H
+#define ORDERED_HASH_MAP_H
+/*************************************************************************/
+/*  ordered_hash_map.h                                                   */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "core/containers/og_hash_map.h"
+#include "core/containers/list.h"
+#include "core/containers/pair.h"
+
+/**
+ * A hash map which allows to iterate elements in insertion order.
+ * Insertion, lookup, deletion have O(1) complexity.
+ * The API aims to be consistent with Map rather than HashMap, because the
+ * former is more frequently used and is more coherent with the rest of the
+ * codebase.
+ * Deletion during iteration is safe and will preserve the order.
+ */
+template <class K, class V, class Hasher = HashMapHasherDefault, class Comparator = HashMapComparatorDefault<K>, uint8_t MIN_HASH_TABLE_POWER = 3, uint8_t RELATIONSHIP = 8>
+class OrderedHashMap {
+	typedef List<Pair<const K *, V>> InternalList;
+	typedef OGHashMap<K, typename InternalList::Element *, Hasher, Comparator, MIN_HASH_TABLE_POWER, RELATIONSHIP> InternalMap;
+
+	InternalList list;
+	InternalMap map;
+
+public:
+	class Element {
+		friend class OrderedHashMap<K, V, Hasher, Comparator, MIN_HASH_TABLE_POWER, RELATIONSHIP>;
+
+		typename InternalList::Element *list_element;
+		typename InternalList::Element *prev_element;
+		typename InternalList::Element *next_element;
+
+		Element(typename InternalList::Element *p_element) {
+			list_element = p_element;
+
+			if (list_element) {
+				next_element = list_element->next();
+				prev_element = list_element->prev();
+			}
+		}
+
+	public:
+		_FORCE_INLINE_ Element() :
+				list_element(nullptr),
+				prev_element(nullptr),
+				next_element(nullptr) {
+		}
+
+		Element next() const {
+			return Element(next_element);
+		}
+
+		Element prev() const {
+			return Element(prev_element);
+		}
+
+		Element(const Element &other) :
+				list_element(other.list_element),
+				prev_element(other.prev_element),
+				next_element(other.next_element) {
+		}
+
+		Element &operator=(const Element &other) {
+			list_element = other.list_element;
+			next_element = other.next_element;
+			prev_element = other.prev_element;
+			return *this;
+		}
+
+		_FORCE_INLINE_ bool operator==(const Element &p_other) const {
+			return this->list_element == p_other.list_element;
+		}
+		_FORCE_INLINE_ bool operator!=(const Element &p_other) const {
+			return this->list_element != p_other.list_element;
+		}
+
+		operator bool() const {
+			return (list_element != nullptr);
+		}
+
+		const K &key() const {
+			CRASH_COND(!list_element);
+			return *(list_element->get().first);
+		};
+
+		V &value() {
+			CRASH_COND(!list_element);
+			return list_element->get().second;
+		};
+
+		const V &value() const {
+			CRASH_COND(!list_element);
+			return list_element->get().second;
+		};
+
+		V &get() {
+			CRASH_COND(!list_element);
+			return list_element->get().second;
+		};
+
+		const V &get() const {
+			CRASH_COND(!list_element);
+			return list_element->get().second;
+		};
+	};
+
+	class ConstElement {
+		friend class OrderedHashMap<K, V, Hasher, Comparator, MIN_HASH_TABLE_POWER, RELATIONSHIP>;
+
+		const typename InternalList::Element *list_element;
+
+		ConstElement(const typename InternalList::Element *p_element) :
+				list_element(p_element) {
+		}
+
+	public:
+		_FORCE_INLINE_ ConstElement() :
+				list_element(NULL) {
+		}
+
+		ConstElement(const ConstElement &other) :
+				list_element(other.list_element) {
+		}
+
+		ConstElement &operator=(const ConstElement &other) {
+			list_element = other.list_element;
+			return *this;
+		}
+
+		ConstElement next() const {
+			return ConstElement(list_element ? list_element->next() : nullptr);
+		}
+
+		ConstElement prev() const {
+			return ConstElement(list_element ? list_element->prev() : NULL);
+		}
+
+		_FORCE_INLINE_ bool operator==(const ConstElement &p_other) const {
+			return this->list_element == p_other.list_element;
+		}
+		_FORCE_INLINE_ bool operator!=(const ConstElement &p_other) const {
+			return this->list_element != p_other.list_element;
+		}
+
+		operator bool() const {
+			return (list_element != nullptr);
+		}
+
+		const K &key() const {
+			CRASH_COND(!list_element);
+			return *(list_element->get().first);
+		};
+
+		const V &value() const {
+			CRASH_COND(!list_element);
+			return list_element->get().second;
+		};
+
+		const V &get() const {
+			CRASH_COND(!list_element);
+			return list_element->get().second;
+		};
+	};
+
+	ConstElement find(const K &p_key) const {
+		typename InternalList::Element *const *list_element = map.getptr(p_key);
+		if (list_element) {
+			return ConstElement(*list_element);
+		}
+		return ConstElement(nullptr);
+	}
+
+	Element find(const K &p_key) {
+		typename InternalList::Element **list_element = map.getptr(p_key);
+		if (list_element) {
+			return Element(*list_element);
+		}
+		return Element(nullptr);
+	}
+
+	Element insert(const K &p_key, const V &p_value) {
+		typename InternalList::Element **list_element = map.getptr(p_key);
+		if (list_element) {
+			(*list_element)->get().second = p_value;
+			return Element(*list_element);
+		}
+		// Incorrectly set the first value of the pair with a value that will
+		// be invalid as soon as we leave this function...
+		typename InternalList::Element *new_element = list.push_back(Pair<const K *, V>(&p_key, p_value));
+		// ...this is needed here in case the hashmap recursively reference itself...
+		typename InternalMap::Element *e = map.set(p_key, new_element);
+		// ...now we can set the right value !
+		new_element->get().first = &e->key();
+
+		return Element(new_element);
+	}
+
+	void erase(Element &p_element) {
+		map.erase(p_element.key());
+		list.erase(p_element.list_element);
+		p_element.list_element = nullptr;
+	}
+
+	bool erase(const K &p_key) {
+		typename InternalList::Element **list_element = map.getptr(p_key);
+		if (list_element) {
+			list.erase(*list_element);
+			map.erase(p_key);
+			return true;
+		}
+		return false;
+	}
+
+	inline bool has(const K &p_key) const {
+		return map.has(p_key);
+	}
+
+	const V &operator[](const K &p_key) const {
+		ConstElement e = find(p_key);
+		CRASH_COND(!e);
+		return e.value();
+	}
+
+	V &operator[](const K &p_key) {
+		Element e = find(p_key);
+		if (!e) {
+			// consistent with Map behaviour
+			e = insert(p_key, V());
+		}
+		return e.value();
+	}
+
+	inline Element front() {
+		return Element(list.front());
+	}
+
+	inline Element back() {
+		return Element(list.back());
+	}
+
+	inline ConstElement front() const {
+		return ConstElement(list.front());
+	}
+
+	inline ConstElement back() const {
+		return ConstElement(list.back());
+	}
+
+	inline bool empty() const { return list.empty(); }
+	inline int size() const { return list.size(); }
+
+	const void *id() const {
+		return list.id();
+	}
+
+	void clear() {
+		map.clear();
+		list.clear();
+	}
+
+private:
+	void _copy_from(const OrderedHashMap &p_map) {
+		for (ConstElement E = p_map.front(); E; E = E.next()) {
+			insert(E.key(), E.value());
+		}
+	}
+
+public:
+	void operator=(const OrderedHashMap &p_map) {
+		_copy_from(p_map);
+	}
+
+	OrderedHashMap(const OrderedHashMap &p_map) {
+		_copy_from(p_map);
+	}
+
+	_FORCE_INLINE_ OrderedHashMap() {
+	}
+};
+
+#endif // ORDERED_HASH_MAP_H

core/containers/paged_allocator.h

@@ -0,0 +1,132 @@
+#ifndef PAGED_ALLOCATOR_H
+#define PAGED_ALLOCATOR_H
+/*************************************************************************/
+/*  paged_allocator.h                                                    */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "core/os/memory.h"
+#include "core/os/spin_lock.h"
+#include "core/typedefs.h"
+
+template <class T, bool thread_safe = false>
+class PagedAllocator {
+	T **page_pool = nullptr;
+	T ***available_pool = nullptr;
+	uint32_t pages_allocated = 0;
+	uint32_t allocs_available = 0;
+
+	uint32_t page_shift = 0;
+	uint32_t page_mask = 0;
+	uint32_t page_size = 0;
+	SpinLock spin_lock;
+
+public:
+	T *alloc() {
+		if (thread_safe) {
+			spin_lock.lock();
+		}
+		if (unlikely(allocs_available == 0)) {
+			uint32_t pages_used = pages_allocated;
+
+			pages_allocated++;
+			page_pool = (T **)memrealloc(page_pool, sizeof(T *) * pages_allocated);
+			available_pool = (T ***)memrealloc(available_pool, sizeof(T **) * pages_allocated);
+
+			page_pool[pages_used] = (T *)memalloc(sizeof(T) * page_size);
+			available_pool[pages_used] = (T **)memalloc(sizeof(T *) * page_size);
+
+			for (uint32_t i = 0; i < page_size; i++) {
+				available_pool[0][i] = &page_pool[pages_used][i];
+			}
+			allocs_available += page_size;
+		}
+
+		allocs_available--;
+		T *alloc = available_pool[allocs_available >> page_shift][allocs_available & page_mask];
+		if (thread_safe) {
+			spin_lock.unlock();
+		}
+		memnew_placement(alloc, T);
+		return alloc;
+	}
+
+	void free(T *p_mem) {
+		if (thread_safe) {
+			spin_lock.lock();
+		}
+		p_mem->~T();
+		available_pool[allocs_available >> page_shift][allocs_available & page_mask] = p_mem;
+		if (thread_safe) {
+			spin_lock.unlock();
+		}
+		allocs_available++;
+	}
+
+	void reset(bool p_allow_unfreed = false) {
+		if (!p_allow_unfreed || !HAS_TRIVIAL_DESTRUCTOR(T)) {
+			ERR_FAIL_COND(allocs_available < pages_allocated * page_size);
+		}
+		if (pages_allocated) {
+			for (uint32_t i = 0; i < pages_allocated; i++) {
+				memfree(page_pool[i]);
+				memfree(available_pool[i]);
+			}
+			memfree(page_pool);
+			memfree(available_pool);
+			page_pool = nullptr;
+			available_pool = nullptr;
+			pages_allocated = 0;
+			allocs_available = 0;
+		}
+	}
+	bool is_configured() const {
+		return page_size > 0;
+	}
+
+	void configure(uint32_t p_page_size) {
+		ERR_FAIL_COND(page_pool != nullptr); //sanity check
+		ERR_FAIL_COND(p_page_size == 0);
+		page_size = nearest_power_of_2_templated(p_page_size);
+		page_mask = page_size - 1;
+		page_shift = get_shift_from_power_of_2(page_size);
+	}
+
+	// Power of 2 recommended because of alignment with OS page sizes. 
+	// Even if element is bigger, its still a multiple and get rounded amount of pages
+	PagedAllocator(uint32_t p_page_size = 4096) { 
+		configure(p_page_size);
+	}
+
+	~PagedAllocator() {
+		ERR_FAIL_COND_MSG(allocs_available < pages_allocated * page_size, "Pages in use exist at exit in PagedAllocator");
+		reset();
+	}
+};
+
+#endif // PAGED_ALLOCATOR_H

core/containers/paged_array.h

@@ -0,0 +1,367 @@
+/**************************************************************************/
+/*  paged_array.h                                                         */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef PAGED_ARRAY_H
+#define PAGED_ARRAY_H
+
+#include "core/os/memory.h"
+#include "core/os/spin_lock.h"
+#include "core/typedefs.h"
+
+// PagedArray is used mainly for filling a very large array from multiple threads efficiently and without causing major fragmentation
+
+// PageArrayPool manages central page allocation in a thread safe matter
+
+template <class T>
+class PagedArrayPool {
+	T **page_pool = nullptr;
+	uint32_t pages_allocated = 0;
+
+	uint32_t *available_page_pool = nullptr;
+	uint32_t pages_available = 0;
+
+	uint32_t page_size = 0;
+	SpinLock spin_lock;
+
+public:
+	uint32_t alloc_page() {
+		spin_lock.lock();
+		if (unlikely(pages_available == 0)) {
+			uint32_t pages_used = pages_allocated;
+
+			pages_allocated++;
+			page_pool = (T **)memrealloc(page_pool, sizeof(T *) * pages_allocated);
+			available_page_pool = (uint32_t *)memrealloc(available_page_pool, sizeof(uint32_t) * pages_allocated);
+
+			page_pool[pages_used] = (T *)memalloc(sizeof(T) * page_size);
+			available_page_pool[0] = pages_used;
+
+			pages_available++;
+		}
+
+		pages_available--;
+		uint32_t page = available_page_pool[pages_available];
+		spin_lock.unlock();
+
+		return page;
+	}
+	T *get_page(uint32_t p_page_id) {
+		return page_pool[p_page_id];
+	}
+
+	void free_page(uint32_t p_page_id) {
+		spin_lock.lock();
+		available_page_pool[pages_available] = p_page_id;
+		pages_available++;
+		spin_lock.unlock();
+	}
+
+	uint32_t get_page_size_shift() const {
+		return get_shift_from_power_of_2(page_size);
+	}
+
+	uint32_t get_page_size_mask() const {
+		return page_size - 1;
+	}
+
+	void reset() {
+		ERR_FAIL_COND(pages_available < pages_allocated);
+		if (pages_allocated) {
+			for (uint32_t i = 0; i < pages_allocated; i++) {
+				memfree(page_pool[i]);
+			}
+			memfree(page_pool);
+			memfree(available_page_pool);
+			page_pool = nullptr;
+			available_page_pool = nullptr;
+			pages_allocated = 0;
+			pages_available = 0;
+		}
+	}
+	bool is_configured() const {
+		return page_size > 0;
+	}
+
+	void configure(uint32_t p_page_size) {
+		ERR_FAIL_COND(page_pool != nullptr); //sanity check
+		ERR_FAIL_COND(p_page_size == 0);
+		page_size = nearest_power_of_2_templated(p_page_size);
+	}
+
+	PagedArrayPool(uint32_t p_page_size = 4096) { // power of 2 recommended because of alignment with OS page sizes. Even if element is bigger, its still a multiple and get rounded amount of pages
+		configure(p_page_size);
+	}
+
+	~PagedArrayPool() {
+		ERR_FAIL_COND_MSG(pages_available < pages_allocated, "Pages in use exist at exit in PagedArrayPool");
+		reset();
+	}
+};
+
+// PageArray is a local array that is optimized to grow in place, then be cleared often.
+// It does so by allocating pages from a PagedArrayPool.
+// It is safe to use multiple PagedArrays from different threads, sharing a single PagedArrayPool
+
+template <class T>
+class PagedArray {
+	PagedArrayPool<T> *page_pool = nullptr;
+
+	T **page_data = nullptr;
+	uint32_t *page_ids = nullptr;
+	uint32_t max_pages_used = 0;
+	uint32_t page_size_shift = 0;
+	uint32_t page_size_mask = 0;
+	uint64_t count = 0;
+
+	_FORCE_INLINE_ uint32_t _get_pages_in_use() const {
+		if (count == 0) {
+			return 0;
+		} else {
+			return ((count - 1) >> page_size_shift) + 1;
+		}
+	}
+
+	void _grow_page_array() {
+		//no more room in the page array to put the new page, make room
+		if (max_pages_used == 0) {
+			max_pages_used = 1;
+		} else {
+			max_pages_used *= 2; // increase in powers of 2 to keep allocations to minimum
+		}
+		page_data = (T **)memrealloc(page_data, sizeof(T *) * max_pages_used);
+		page_ids = (uint32_t *)memrealloc(page_ids, sizeof(uint32_t) * max_pages_used);
+	}
+
+public:
+	_FORCE_INLINE_ const T &operator[](uint64_t p_index) const {
+		CRASH_BAD_UNSIGNED_INDEX(p_index, count);
+		uint32_t page = p_index >> page_size_shift;
+		uint32_t offset = p_index & page_size_mask;
+
+		return page_data[page][offset];
+	}
+	_FORCE_INLINE_ T &operator[](uint64_t p_index) {
+		CRASH_BAD_UNSIGNED_INDEX(p_index, count);
+		uint32_t page = p_index >> page_size_shift;
+		uint32_t offset = p_index & page_size_mask;
+
+		return page_data[page][offset];
+	}
+
+	_FORCE_INLINE_ void push_back(const T &p_value) {
+		uint32_t remainder = count & page_size_mask;
+		if (unlikely(remainder == 0)) {
+			// at 0, so time to request a new page
+			uint32_t page_count = _get_pages_in_use();
+			uint32_t new_page_count = page_count + 1;
+
+			if (unlikely(new_page_count > max_pages_used)) {
+				ERR_FAIL_COND(page_pool == nullptr); //sanity check
+
+				_grow_page_array(); //keep out of inline
+			}
+
+			uint32_t page_id = page_pool->alloc_page();
+			page_data[page_count] = page_pool->get_page(page_id);
+			page_ids[page_count] = page_id;
+		}
+
+		// place the new value
+		uint32_t page = count >> page_size_shift;
+		uint32_t offset = count & page_size_mask;
+
+		if (!HAS_TRIVIAL_CONSTRUCTOR(T)) {
+			memnew_placement(&page_data[page][offset], T(p_value));
+		} else {
+			page_data[page][offset] = p_value;
+		}
+
+		count++;
+	}
+
+	_FORCE_INLINE_ void pop_back() {
+		ERR_FAIL_COND(count == 0);
+
+		if (!HAS_TRIVIAL_DESTRUCTOR(T)) {
+			uint32_t page = (count - 1) >> page_size_shift;
+			uint32_t offset = (count - 1) & page_size_mask;
+			page_data[page][offset].~T();
+		}
+
+		uint32_t remainder = count & page_size_mask;
+		if (unlikely(remainder == 1)) {
+			// one element remained, so page must be freed.
+			uint32_t last_page = _get_pages_in_use() - 1;
+			page_pool->free_page(page_ids[last_page]);
+		}
+		count--;
+	}
+
+	void clear() {
+		//destruct if needed
+		if (!HAS_TRIVIAL_DESTRUCTOR(T)) {
+			for (uint64_t i = 0; i < count; i++) {
+				uint32_t page = i >> page_size_shift;
+				uint32_t offset = i & page_size_mask;
+				page_data[page][offset].~T();
+			}
+		}
+
+		//return the pages to the pagepool, so they can be used by another array eventually
+		uint32_t pages_used = _get_pages_in_use();
+		for (uint32_t i = 0; i < pages_used; i++) {
+			page_pool->free_page(page_ids[i]);
+		}
+
+		count = 0;
+
+		//note we leave page_data and page_indices intact for next use. If you really want to clear them call reset()
+	}
+
+	void reset() {
+		clear();
+		if (page_data) {
+			memfree(page_data);
+			memfree(page_ids);
+			page_data = nullptr;
+			page_ids = nullptr;
+			max_pages_used = 0;
+		}
+	}
+
+	// This takes the pages from a source array and merges them to this one
+	// resulting order is undefined, but content is merged very efficiently,
+	// making it ideal to fill content on several threads to later join it.
+
+	void merge_unordered(PagedArray<T> &p_array) {
+		ERR_FAIL_COND(page_pool != p_array.page_pool);
+
+		uint32_t remainder = count & page_size_mask;
+
+		T *remainder_page = nullptr;
+		uint32_t remainder_page_id = 0;
+
+		if (remainder > 0) {
+			uint32_t last_page = _get_pages_in_use() - 1;
+			remainder_page = page_data[last_page];
+			remainder_page_id = page_ids[last_page];
+		}
+
+		count -= remainder;
+
+		uint32_t src_pages = p_array._get_pages_in_use();
+		uint32_t page_size = page_size_mask + 1;
+
+		for (uint32_t i = 0; i < src_pages; i++) {
+			uint32_t page_count = _get_pages_in_use();
+			uint32_t new_page_count = page_count + 1;
+
+			if (unlikely(new_page_count > max_pages_used)) {
+				_grow_page_array(); //keep out of inline
+			}
+
+			page_data[page_count] = p_array.page_data[i];
+			page_ids[page_count] = p_array.page_ids[i];
+			if (i == src_pages - 1) {
+				//last page, only increment with remainder
+				count += p_array.count & page_size_mask;
+			} else {
+				count += page_size;
+			}
+		}
+		p_array.count = 0; //take away the other array pages
+
+		//handle the remainder page if exists
+		if (remainder_page) {
+			uint32_t new_remainder = count & page_size_mask;
+
+			if (new_remainder > 0) {
+				//must merge old remainder with new remainder
+
+				T *dst_page = page_data[_get_pages_in_use() - 1];
+				uint32_t to_copy = MIN(page_size - new_remainder, remainder);
+
+				for (uint32_t i = 0; i < to_copy; i++) {
+					if (!HAS_TRIVIAL_CONSTRUCTOR(T)) {
+						memnew_placement(&dst_page[i + new_remainder], T(remainder_page[i + remainder - to_copy]));
+					} else {
+						dst_page[i + new_remainder] = remainder_page[i + remainder - to_copy];
+					}
+
+					if (!HAS_TRIVIAL_DESTRUCTOR(T)) {
+						remainder_page[i + remainder - to_copy].~T();
+					}
+				}
+
+				remainder -= to_copy; //subtract what was copied from remainder
+				count += to_copy; //add what was copied to the count
+
+				if (remainder == 0) {
+					//entire remainder copied, let go of remainder page
+					page_pool->free_page(remainder_page_id);
+					remainder_page = nullptr;
+				}
+			}
+
+			if (remainder > 0) {
+				//there is still remainder, append it
+				uint32_t page_count = _get_pages_in_use();
+				uint32_t new_page_count = page_count + 1;
+
+				if (unlikely(new_page_count > max_pages_used)) {
+					_grow_page_array(); //keep out of inline
+				}
+
+				page_data[page_count] = remainder_page;
+				page_ids[page_count] = remainder_page_id;
+
+				count += remainder;
+			}
+		}
+	}
+
+	_FORCE_INLINE_ uint64_t size() const {
+		return count;
+	}
+
+	void set_page_pool(PagedArrayPool<T> *p_page_pool) {
+		ERR_FAIL_COND(max_pages_used > 0); //sanity check
+
+		page_pool = p_page_pool;
+		page_size_mask = page_pool->get_page_size_mask();
+		page_size_shift = page_pool->get_page_size_shift();
+	}
+
+	~PagedArray() {
+		reset();
+	}
+};
+
+#endif // PAGED_ARRAY_H

core/containers/pair.h

@@ -0,0 +1,114 @@
+#ifndef PAIR_H
+#define PAIR_H
+/*************************************************************************/
+/*  pair.h                                                               */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "core/containers/hashfuncs.h"
+#include "core/typedefs.h"
+
+template <class F, class S>
+struct Pair {
+	F first;
+	S second;
+
+	Pair() :
+			first(),
+			second() {
+	}
+
+	Pair(F p_first, const S &p_second) :
+			first(p_first),
+			second(p_second) {
+	}
+};
+
+template <class F, class S>
+bool operator==(const Pair<F, S> &pair, const Pair<F, S> &other) {
+	return (pair.first == other.first) && (pair.second == other.second);
+}
+
+template <class F, class S>
+bool operator!=(const Pair<F, S> &pair, const Pair<F, S> &other) {
+	return (pair.first != other.first) || (pair.second != other.second);
+}
+
+template <class F, class S>
+struct PairSort {
+	bool operator()(const Pair<F, S> &A, const Pair<F, S> &B) const {
+		if (A.first != B.first) {
+			return A.first < B.first;
+		}
+		return A.second < B.second;
+	}
+};
+
+template <class F, class S>
+struct PairHash {
+	static uint32_t hash(const Pair<F, S> &P) {
+		uint64_t h1 = HashMapHasherDefault::hash(P.first);
+		uint64_t h2 = HashMapHasherDefault::hash(P.second);
+		return hash_one_uint64((h1 << 32) | h2);
+	}
+};
+
+template <class K, class V>
+struct KeyValue {
+	const K key;
+	V value;
+
+	void operator=(const KeyValue &p_kv) = delete;
+	_FORCE_INLINE_ KeyValue(const KeyValue &p_kv) :
+			key(p_kv.key),
+			value(p_kv.value) {
+	}
+	_FORCE_INLINE_ KeyValue(const K &p_key, const V &p_value) :
+			key(p_key),
+			value(p_value) {
+	}
+};
+
+template <class K, class V>
+bool operator==(const KeyValue<K, V> &pair, const KeyValue<K, V> &other) {
+	return (pair.key == other.key) && (pair.value == other.value);
+}
+
+template <class K, class V>
+bool operator!=(const KeyValue<K, V> &pair, const KeyValue<K, V> &other) {
+	return (pair.key != other.key) || (pair.value != other.value);
+}
+
+template <class K, class V>
+struct KeyValueSort {
+	bool operator()(const KeyValue<K, V> &A, const KeyValue<K, V> &B) const {
+		return A.key < B.key;
+	}
+};
+
+#endif // PAIR_H

core/containers/pooled_list.h

@@ -0,0 +1,212 @@
+
+#ifndef POOLED_LIST_H
+#define POOLED_LIST_H
+
+/*************************************************************************/
+/*  pooled_list.h                                                        */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+// Simple template to provide a pool with O(1) allocate and free.
+// The freelist could alternatively be a linked list placed within the unused elements
+// to use less memory, however a separate freelist is probably more cache friendly.
+
+// NOTE : Take great care when using this with non POD types. The construction and destruction
+// is done in the LocalVector, NOT as part of the pool. So requesting a new item does not guarantee
+// a constructor is run, and free does not guarantee a destructor.
+// You should generally handle clearing
+// an item explicitly after a request, as it may contain 'leftovers'.
+// This is by design for fastest use in the BVH. If you want a more general pool
+// that does call constructors / destructors on request / free, this should probably be
+// a separate template.
+
+// The zero_on_first_request feature is optional and is useful for e.g. pools of handles,
+// which may use a ref count which we want to be initialized to zero the first time a handle is created,
+// but left alone on subsequent allocations (as will typically be incremented).
+
+// Note that there is no function to compact the pool - this would
+// invalidate any existing pool IDs held externally.
+// Compaction can be done but would rely on a more complex method
+// of preferentially giving out lower IDs in the freelist first.
+
+#include "core/containers/local_vector.h"
+
+template <class T, class U = uint32_t, bool force_trivial = false, bool zero_on_first_request = false>
+class PooledList {
+	LocalVector<T, U, force_trivial> list;
+	LocalVector<U, U, true> freelist;
+
+	// not all list members are necessarily used
+	U _used_size;
+
+public:
+	PooledList() {
+		_used_size = 0;
+	}
+
+	// Use with care, in most cases you should make sure to
+	// free all elements first (i.e. _used_size would be zero),
+	// although it could also be used without this as an optimization
+	// in some cases.
+	void clear() {
+		list.clear();
+		freelist.clear();
+		_used_size = 0;
+	}
+
+	uint64_t estimate_memory_use() const {
+		return ((uint64_t)list.size() * sizeof(T)) + ((uint64_t)freelist.size() * sizeof(U));
+	}
+
+	const T &operator[](U p_index) const {
+		return list[p_index];
+	}
+	T &operator[](U p_index) {
+		return list[p_index];
+	}
+
+	// To be explicit in a pool there is a distinction
+	// between the number of elements that are currently
+	// in use, and the number of elements that have been reserved.
+	// Using size() would be vague.
+	U used_size() const { return _used_size; }
+	U reserved_size() const { return list.size(); }
+
+	T *request(U &r_id) {
+		_used_size++;
+
+		if (freelist.size()) {
+			// pop from freelist
+			int new_size = freelist.size() - 1;
+			r_id = freelist[new_size];
+			freelist.resize(new_size);
+
+			return &list[r_id];
+		}
+
+		r_id = list.size();
+		list.resize(r_id + 1);
+
+		static_assert((!zero_on_first_request) || (__is_pod(T)), "zero_on_first_request requires trivial type");
+		if (zero_on_first_request && __is_pod(T)) {
+			list[r_id] = {};
+		}
+
+		return &list[r_id];
+	}
+	void free(const U &p_id) {
+		// should not be on free list already
+		ERR_FAIL_UNSIGNED_INDEX(p_id, list.size());
+		freelist.push_back(p_id);
+		ERR_FAIL_COND_MSG(!_used_size, "_used_size has become out of sync, have you double freed an item?");
+		_used_size--;
+	}
+};
+
+// a pooled list which automatically keeps a list of the active members
+template <class T, class U = uint32_t, bool force_trivial = false, bool zero_on_first_request = false>
+class TrackedPooledList {
+public:
+	U pool_used_size() const { return _pool.used_size(); }
+	U pool_reserved_size() const { return _pool.reserved_size(); }
+	U active_size() const { return _active_list.size(); }
+
+	// use with care, see the earlier notes in the PooledList clear()
+	void clear() {
+		_pool.clear();
+		_active_list.clear();
+		_active_map.clear();
+	}
+
+	U get_active_id(U p_index) const {
+		return _active_list[p_index];
+	}
+
+	const T &get_active(U p_index) const {
+		return _pool[get_active_id(p_index)];
+	}
+
+	T &get_active(U p_index) {
+		return _pool[get_active_id(p_index)];
+	}
+
+	const T &operator[](U p_index) const {
+		return _pool[p_index];
+	}
+	T &operator[](U p_index) {
+		return _pool[p_index];
+	}
+
+	T *request(U &r_id) {
+		T *item = _pool.request(r_id);
+
+		// add to the active list
+		U active_list_id = _active_list.size();
+		_active_list.push_back(r_id);
+
+		// expand the active map (this should be in sync with the pool list
+		if (_pool.used_size() > _active_map.size()) {
+			_active_map.resize(_pool.used_size());
+		}
+
+		// store in the active map
+		_active_map[r_id] = active_list_id;
+
+		return item;
+	}
+
+	void free(const U &p_id) {
+		_pool.free(p_id);
+
+		// remove from the active list.
+		U list_id = _active_map[p_id];
+
+		// zero the _active map to detect bugs (only in debug?)
+		_active_map[p_id] = -1;
+
+		_active_list.remove_unordered(list_id);
+
+		// keep the replacement in sync with the correct list Id
+		if (list_id < _active_list.size()) {
+			// which pool id has been replaced in the active list
+			U replacement_id = _active_list[list_id];
+
+			// keep that replacements map up to date with the new position
+			_active_map[replacement_id] = list_id;
+		}
+	}
+
+	const LocalVector<U, U> &get_active_list() const { return _active_list; }
+
+private:
+	PooledList<T, U, force_trivial, zero_on_first_request> _pool;
+	LocalVector<U, U> _active_map;
+	LocalVector<U, U> _active_list;
+};
+
+#endif

core/containers/rb_map.h

@@ -0,0 +1,678 @@
+#ifndef RB_MAP_H
+#define RB_MAP_H
+
+/*************************************************************************/
+/*  map.h                                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "core/error/error_macros.h"
+#include "core/os/memory.h"
+
+// based on the very nice implementation of rb-trees by:
+// https://web.archive.org/web/20120507164830/http://web.mit.edu/~emin/www/source_code/red_black_tree/index.html
+
+template <class K, class V, class C = Comparator<K>, class A = DefaultAllocator>
+class RBMap {
+	enum Color {
+		RED,
+		BLACK
+	};
+	struct _Data;
+
+public:
+	class Element {
+	private:
+		friend class RBMap<K, V, C, A>;
+		int color;
+		Element *right;
+		Element *left;
+		Element *parent;
+		Element *_next;
+		Element *_prev;
+		K _key;
+		V _value;
+		//_Data *data;
+
+	public:
+		const Element *next() const {
+			return _next;
+		}
+		Element *next() {
+			return _next;
+		}
+		const Element *prev() const {
+			return _prev;
+		}
+		Element *prev() {
+			return _prev;
+		}
+		const K &key() const {
+			return _key;
+		};
+		V &value() {
+			return _value;
+		};
+		const V &value() const {
+			return _value;
+		};
+		V &get() {
+			return _value;
+		};
+		const V &get() const {
+			return _value;
+		};
+		Element() {
+			color = RED;
+			right = nullptr;
+			left = nullptr;
+			parent = nullptr;
+			_next = nullptr;
+			_prev = nullptr;
+		};
+	};
+
+private:
+	struct _Data {
+		Element *_root;
+		Element *_nil;
+		int size_cache;
+
+		_FORCE_INLINE_ _Data() {
+#ifdef GLOBALNIL_DISABLED
+			_nil = memnew_allocator(Element, A);
+			_nil->parent = _nil->left = _nil->right = _nil;
+			_nil->color = BLACK;
+#else
+			_nil = (Element *)&_GlobalNilClass::_nil;
+#endif
+			_root = nullptr;
+			size_cache = 0;
+		}
+
+		void _create_root() {
+			_root = memnew_allocator(Element, A);
+			_root->parent = _root->left = _root->right = _nil;
+			_root->color = BLACK;
+		}
+
+		void _free_root() {
+			if (_root) {
+				memdelete_allocator<Element, A>(_root);
+				_root = nullptr;
+			}
+		}
+
+		~_Data() {
+			_free_root();
+
+#ifdef GLOBALNIL_DISABLED
+			memdelete_allocator<Element, A>(_nil);
+#endif
+		}
+	};
+
+	_Data _data;
+
+	inline void _set_color(Element *p_node, int p_color) {
+		ERR_FAIL_COND(p_node == _data._nil && p_color == RED);
+		p_node->color = p_color;
+	}
+
+	inline void _rotate_left(Element *p_node) {
+		Element *r = p_node->right;
+		p_node->right = r->left;
+		if (r->left != _data._nil) {
+			r->left->parent = p_node;
+		}
+		r->parent = p_node->parent;
+		if (p_node == p_node->parent->left) {
+			p_node->parent->left = r;
+		} else {
+			p_node->parent->right = r;
+		}
+
+		r->left = p_node;
+		p_node->parent = r;
+	}
+
+	inline void _rotate_right(Element *p_node) {
+		Element *l = p_node->left;
+		p_node->left = l->right;
+		if (l->right != _data._nil) {
+			l->right->parent = p_node;
+		}
+		l->parent = p_node->parent;
+		if (p_node == p_node->parent->right) {
+			p_node->parent->right = l;
+		} else {
+			p_node->parent->left = l;
+		}
+
+		l->right = p_node;
+		p_node->parent = l;
+	}
+
+	inline Element *_successor(Element *p_node) const {
+		Element *node = p_node;
+
+		if (node->right != _data._nil) {
+			node = node->right;
+			while (node->left != _data._nil) { /* returns the minimum of the right subtree of node */
+				node = node->left;
+			}
+			return node;
+		} else {
+			while (node == node->parent->right) {
+				node = node->parent;
+			}
+
+			if (node->parent == _data._root) {
+				return nullptr; // No successor, as p_node = last node
+			}
+			return node->parent;
+		}
+	}
+
+	inline Element *_predecessor(Element *p_node) const {
+		Element *node = p_node;
+
+		if (node->left != _data._nil) {
+			node = node->left;
+			while (node->right != _data._nil) { /* returns the minimum of the left subtree of node */
+				node = node->right;
+			}
+			return node;
+		} else {
+			while (node == node->parent->left) {
+				node = node->parent;
+			}
+
+			if (node == _data._root) {
+				return nullptr; // No predecessor, as p_node = first node
+			}
+			return node->parent;
+		}
+	}
+
+	Element *_find(const K &p_key) const {
+		Element *node = _data._root->left;
+		C less;
+
+		while (node != _data._nil) {
+			if (less(p_key, node->_key)) {
+				node = node->left;
+			} else if (less(node->_key, p_key)) {
+				node = node->right;
+			} else {
+				return node; // found
+			}
+		}
+
+		return nullptr;
+	}
+
+	Element *_find_closest(const K &p_key) const {
+		Element *node = _data._root->left;
+		Element *prev = nullptr;
+		C less;
+
+		while (node != _data._nil) {
+			prev = node;
+
+			if (less(p_key, node->_key)) {
+				node = node->left;
+			} else if (less(node->_key, p_key)) {
+				node = node->right;
+			} else {
+				return node; // found
+			}
+		}
+
+		if (prev == nullptr) {
+			return nullptr; // tree empty
+		}
+
+		if (less(p_key, prev->_key)) {
+			prev = prev->_prev;
+		}
+
+		return prev;
+	}
+
+	void _insert_rb_fix(Element *p_new_node) {
+		Element *node = p_new_node;
+		Element *nparent = node->parent;
+		Element *ngrand_parent;
+
+		while (nparent->color == RED) {
+			ngrand_parent = nparent->parent;
+
+			if (nparent == ngrand_parent->left) {
+				if (ngrand_parent->right->color == RED) {
+					_set_color(nparent, BLACK);
+					_set_color(ngrand_parent->right, BLACK);
+					_set_color(ngrand_parent, RED);
+					node = ngrand_parent;
+					nparent = node->parent;
+				} else {
+					if (node == nparent->right) {
+						_rotate_left(nparent);
+						node = nparent;
+						nparent = node->parent;
+					}
+					_set_color(nparent, BLACK);
+					_set_color(ngrand_parent, RED);
+					_rotate_right(ngrand_parent);
+				}
+			} else {
+				if (ngrand_parent->left->color == RED) {
+					_set_color(nparent, BLACK);
+					_set_color(ngrand_parent->left, BLACK);
+					_set_color(ngrand_parent, RED);
+					node = ngrand_parent;
+					nparent = node->parent;
+				} else {
+					if (node == nparent->left) {
+						_rotate_right(nparent);
+						node = nparent;
+						nparent = node->parent;
+					}
+					_set_color(nparent, BLACK);
+					_set_color(ngrand_parent, RED);
+					_rotate_left(ngrand_parent);
+				}
+			}
+		}
+
+		_set_color(_data._root->left, BLACK);
+	}
+
+	Element *_insert(const K &p_key, const V &p_value) {
+		Element *new_parent = _data._root;
+		Element *node = _data._root->left;
+		C less;
+
+		while (node != _data._nil) {
+			new_parent = node;
+
+			if (less(p_key, node->_key)) {
+				node = node->left;
+			} else if (less(node->_key, p_key)) {
+				node = node->right;
+			} else {
+				node->_value = p_value;
+				return node; // Return existing node with new value
+			}
+		}
+
+		Element *new_node = memnew_allocator(Element, A);
+		new_node->parent = new_parent;
+		new_node->right = _data._nil;
+		new_node->left = _data._nil;
+		new_node->_key = p_key;
+		new_node->_value = p_value;
+		//new_node->data=_data;
+
+		if (new_parent == _data._root || less(p_key, new_parent->_key)) {
+			new_parent->left = new_node;
+		} else {
+			new_parent->right = new_node;
+		}
+
+		new_node->_next = _successor(new_node);
+		new_node->_prev = _predecessor(new_node);
+		if (new_node->_next) {
+			new_node->_next->_prev = new_node;
+		}
+		if (new_node->_prev) {
+			new_node->_prev->_next = new_node;
+		}
+
+		_data.size_cache++;
+		_insert_rb_fix(new_node);
+		return new_node;
+	}
+
+	void _erase_fix_rb(Element *p_node) {
+		Element *root = _data._root->left;
+		Element *node = _data._nil;
+		Element *sibling = p_node;
+		Element *parent = sibling->parent;
+
+		while (node != root) { // If red node found, will exit at a break
+			if (sibling->color == RED) {
+				_set_color(sibling, BLACK);
+				_set_color(parent, RED);
+				if (sibling == parent->right) {
+					sibling = sibling->left;
+					_rotate_left(parent);
+				} else {
+					sibling = sibling->right;
+					_rotate_right(parent);
+				}
+			}
+			if ((sibling->left->color == BLACK) && (sibling->right->color == BLACK)) {
+				_set_color(sibling, RED);
+				if (parent->color == RED) {
+					_set_color(parent, BLACK);
+					break;
+				} else { // loop: haven't found any red nodes yet
+					node = parent;
+					parent = node->parent;
+					sibling = (node == parent->left) ? parent->right : parent->left;
+				}
+			} else {
+				if (sibling == parent->right) {
+					if (sibling->right->color == BLACK) {
+						_set_color(sibling->left, BLACK);
+						_set_color(sibling, RED);
+						_rotate_right(sibling);
+						sibling = sibling->parent;
+					}
+					_set_color(sibling, parent->color);
+					_set_color(parent, BLACK);
+					_set_color(sibling->right, BLACK);
+					_rotate_left(parent);
+					break;
+				} else {
+					if (sibling->left->color == BLACK) {
+						_set_color(sibling->right, BLACK);
+						_set_color(sibling, RED);
+						_rotate_left(sibling);
+						sibling = sibling->parent;
+					}
+
+					_set_color(sibling, parent->color);
+					_set_color(parent, BLACK);
+					_set_color(sibling->left, BLACK);
+					_rotate_right(parent);
+					break;
+				}
+			}
+		}
+
+		ERR_FAIL_COND(_data._nil->color != BLACK);
+	}
+
+	void _erase(Element *p_node) {
+		Element *rp = ((p_node->left == _data._nil) || (p_node->right == _data._nil)) ? p_node : p_node->_next;
+		Element *node = (rp->left == _data._nil) ? rp->right : rp->left;
+
+		Element *sibling;
+		if (rp == rp->parent->left) {
+			rp->parent->left = node;
+			sibling = rp->parent->right;
+		} else {
+			rp->parent->right = node;
+			sibling = rp->parent->left;
+		}
+
+		if (node->color == RED) {
+			node->parent = rp->parent;
+			_set_color(node, BLACK);
+		} else if (rp->color == BLACK && rp->parent != _data._root) {
+			_erase_fix_rb(sibling);
+		}
+
+		if (rp != p_node) {
+			ERR_FAIL_COND(rp == _data._nil);
+
+			rp->left = p_node->left;
+			rp->right = p_node->right;
+			rp->parent = p_node->parent;
+			rp->color = p_node->color;
+			if (p_node->left != _data._nil) {
+				p_node->left->parent = rp;
+			}
+			if (p_node->right != _data._nil) {
+				p_node->right->parent = rp;
+			}
+
+			if (p_node == p_node->parent->left) {
+				p_node->parent->left = rp;
+			} else {
+				p_node->parent->right = rp;
+			}
+		}
+
+		if (p_node->_next) {
+			p_node->_next->_prev = p_node->_prev;
+		}
+		if (p_node->_prev) {
+			p_node->_prev->_next = p_node->_next;
+		}
+
+		memdelete_allocator<Element, A>(p_node);
+		_data.size_cache--;
+		ERR_FAIL_COND(_data._nil->color == RED);
+	}
+
+	void _calculate_depth(Element *p_element, int &max_d, int d) const {
+		if (p_element == _data._nil) {
+			return;
+		}
+
+		_calculate_depth(p_element->left, max_d, d + 1);
+		_calculate_depth(p_element->right, max_d, d + 1);
+
+		if (d > max_d) {
+			max_d = d;
+		}
+	}
+
+	void _cleanup_tree(Element *p_element) {
+		if (p_element == _data._nil) {
+			return;
+		}
+
+		_cleanup_tree(p_element->left);
+		_cleanup_tree(p_element->right);
+		memdelete_allocator<Element, A>(p_element);
+	}
+
+	void _copy_from(const RBMap &p_map) {
+		clear();
+		// not the fastest way, but safeset to write.
+		for (Element *I = p_map.front(); I; I = I->next()) {
+			insert(I->key(), I->value());
+		}
+	}
+
+public:
+	const Element *find(const K &p_key) const {
+		if (!_data._root) {
+			return nullptr;
+		}
+
+		const Element *res = _find(p_key);
+		return res;
+	}
+
+	Element *find(const K &p_key) {
+		if (!_data._root) {
+			return nullptr;
+		}
+
+		Element *res = _find(p_key);
+		return res;
+	}
+
+	const Element *find_closest(const K &p_key) const {
+		if (!_data._root) {
+			return NULL;
+		}
+
+		const Element *res = _find_closest(p_key);
+		return res;
+	}
+
+	Element *find_closest(const K &p_key) {
+		if (!_data._root) {
+			return nullptr;
+		}
+
+		Element *res = _find_closest(p_key);
+		return res;
+	}
+
+	bool has(const K &p_key) const {
+		return find(p_key) != nullptr;
+	}
+
+	Element *insert(const K &p_key, const V &p_value) {
+		if (!_data._root) {
+			_data._create_root();
+		}
+		return _insert(p_key, p_value);
+	}
+
+	void erase(Element *p_element) {
+		if (!_data._root || !p_element) {
+			return;
+		}
+
+		_erase(p_element);
+		if (_data.size_cache == 0 && _data._root) {
+			_data._free_root();
+		}
+	}
+
+	bool erase(const K &p_key) {
+		if (!_data._root) {
+			return false;
+		}
+
+		Element *e = find(p_key);
+		if (!e) {
+			return false;
+		}
+
+		_erase(e);
+		if (_data.size_cache == 0 && _data._root) {
+			_data._free_root();
+		}
+		return true;
+	}
+
+	const V &operator[](const K &p_key) const {
+		CRASH_COND(!_data._root);
+		const Element *e = find(p_key);
+		CRASH_COND(!e);
+		return e->_value;
+	}
+
+	V &operator[](const K &p_key) {
+		if (!_data._root) {
+			_data._create_root();
+		}
+
+		Element *e = find(p_key);
+		if (!e) {
+			e = insert(p_key, V());
+		}
+
+		return e->_value;
+	}
+
+	Element *front() const {
+		if (!_data._root) {
+			return nullptr;
+		}
+
+		Element *e = _data._root->left;
+		if (e == _data._nil) {
+			return nullptr;
+		}
+
+		while (e->left != _data._nil) {
+			e = e->left;
+		}
+
+		return e;
+	}
+
+	Element *back() const {
+		if (!_data._root) {
+			return nullptr;
+		}
+
+		Element *e = _data._root->left;
+		if (e == _data._nil) {
+			return nullptr;
+		}
+
+		while (e->right != _data._nil) {
+			e = e->right;
+		}
+
+		return e;
+	}
+
+	inline bool empty() const { return _data.size_cache == 0; }
+	inline int size() const { return _data.size_cache; }
+
+	int calculate_depth() const {
+		// used for debug mostly
+		if (!_data._root) {
+			return 0;
+		}
+
+		int max_d = 0;
+		_calculate_depth(_data._root->left, max_d, 0);
+		return max_d;
+	}
+
+	void clear() {
+		if (!_data._root) {
+			return;
+		}
+
+		_cleanup_tree(_data._root->left);
+		_data._root->left = _data._nil;
+		_data.size_cache = 0;
+		_data._free_root();
+	}
+
+	void operator=(const RBMap &p_map) {
+		_copy_from(p_map);
+	}
+
+	RBMap(const RBMap &p_map) {
+		_copy_from(p_map);
+	}
+
+	_FORCE_INLINE_ RBMap() {
+	}
+
+	~RBMap() {
+		clear();
+	}
+};
+
+#endif

core/containers/rb_set.h

@@ -0,0 +1,632 @@
+#ifndef RB_SET_H
+#define RB_SET_H
+
+/*************************************************************************/
+/*  set.h                                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "core/os/memory.h"
+#include "core/typedefs.h"
+
+// based on the very nice implementation of rb-trees by:
+// https://web.archive.org/web/20120507164830/http://web.mit.edu/~emin/www/source_code/red_black_tree/index.html
+
+template <class T, class C = Comparator<T>, class A = DefaultAllocator>
+class RBSet {
+	enum Color {
+		RED,
+		BLACK
+	};
+	struct _Data;
+
+public:
+	class Element {
+	private:
+		friend class RBSet<T, C, A>;
+		int color;
+		Element *right;
+		Element *left;
+		Element *parent;
+		Element *_next;
+		Element *_prev;
+		T value;
+		//_Data *data;
+
+	public:
+		const Element *next() const {
+			return _next;
+		}
+		Element *next() {
+			return _next;
+		}
+		const Element *prev() const {
+			return _prev;
+		}
+		Element *prev() {
+			return _prev;
+		}
+		const T &get() const {
+			return value;
+		};
+		Element() {
+			color = RED;
+			right = nullptr;
+			left = nullptr;
+			parent = nullptr;
+			_next = nullptr;
+			_prev = nullptr;
+		};
+	};
+
+private:
+	struct _Data {
+		Element *_root;
+		Element *_nil;
+		int size_cache;
+
+		_FORCE_INLINE_ _Data() {
+#ifdef GLOBALNIL_DISABLED
+			_nil = memnew_allocator(Element, A);
+			_nil->parent = _nil->left = _nil->right = _nil;
+			_nil->color = BLACK;
+#else
+			_nil = (Element *)&_GlobalNilClass::_nil;
+#endif
+			_root = nullptr;
+			size_cache = 0;
+		}
+
+		void _create_root() {
+			_root = memnew_allocator(Element, A);
+			_root->parent = _root->left = _root->right = _nil;
+			_root->color = BLACK;
+		}
+
+		void _free_root() {
+			if (_root) {
+				memdelete_allocator<Element, A>(_root);
+				_root = nullptr;
+			}
+		}
+
+		~_Data() {
+			_free_root();
+
+#ifdef GLOBALNIL_DISABLED
+			memdelete_allocator<Element, A>(_nil);
+#endif
+		}
+	};
+
+	_Data _data;
+
+	inline void _set_color(Element *p_node, int p_color) {
+		ERR_FAIL_COND(p_node == _data._nil && p_color == RED);
+		p_node->color = p_color;
+	}
+
+	inline void _rotate_left(Element *p_node) {
+		Element *r = p_node->right;
+		p_node->right = r->left;
+		if (r->left != _data._nil) {
+			r->left->parent = p_node;
+		}
+		r->parent = p_node->parent;
+		if (p_node == p_node->parent->left) {
+			p_node->parent->left = r;
+		} else {
+			p_node->parent->right = r;
+		}
+
+		r->left = p_node;
+		p_node->parent = r;
+	}
+
+	inline void _rotate_right(Element *p_node) {
+		Element *l = p_node->left;
+		p_node->left = l->right;
+		if (l->right != _data._nil) {
+			l->right->parent = p_node;
+		}
+		l->parent = p_node->parent;
+		if (p_node == p_node->parent->right) {
+			p_node->parent->right = l;
+		} else {
+			p_node->parent->left = l;
+		}
+
+		l->right = p_node;
+		p_node->parent = l;
+	}
+
+	inline Element *_successor(Element *p_node) const {
+		Element *node = p_node;
+
+		if (node->right != _data._nil) {
+			node = node->right;
+			while (node->left != _data._nil) { /* returns the minimum of the right subtree of node */
+				node = node->left;
+			}
+			return node;
+		} else {
+			while (node == node->parent->right) {
+				node = node->parent;
+			}
+
+			if (node->parent == _data._root) {
+				return nullptr; // No successor, as p_node = last node
+			}
+			return node->parent;
+		}
+	}
+
+	inline Element *_predecessor(Element *p_node) const {
+		Element *node = p_node;
+
+		if (node->left != _data._nil) {
+			node = node->left;
+			while (node->right != _data._nil) { /* returns the minimum of the left subtree of node */
+				node = node->right;
+			}
+			return node;
+		} else {
+			while (node == node->parent->left) {
+				node = node->parent;
+			}
+
+			if (node == _data._root) {
+				return nullptr; // No predecessor, as p_node = first node.
+			}
+			return node->parent;
+		}
+	}
+
+	Element *_find(const T &p_value) const {
+		Element *node = _data._root->left;
+		C less;
+
+		while (node != _data._nil) {
+			if (less(p_value, node->value)) {
+				node = node->left;
+			} else if (less(node->value, p_value)) {
+				node = node->right;
+			} else {
+				return node; // found
+			}
+		}
+
+		return nullptr;
+	}
+
+	Element *_lower_bound(const T &p_value) const {
+		Element *node = _data._root->left;
+		Element *prev = nullptr;
+		C less;
+
+		while (node != _data._nil) {
+			prev = node;
+
+			if (less(p_value, node->value)) {
+				node = node->left;
+			} else if (less(node->value, p_value)) {
+				node = node->right;
+			} else {
+				return node; // found
+			}
+		}
+
+		if (prev == nullptr) {
+			return nullptr; // tree empty
+		}
+
+		if (less(prev->value, p_value)) {
+			prev = prev->_next;
+		}
+
+		return prev;
+	}
+
+	void _insert_rb_fix(Element *p_new_node) {
+		Element *node = p_new_node;
+		Element *nparent = node->parent;
+		Element *ngrand_parent;
+
+		while (nparent->color == RED) {
+			ngrand_parent = nparent->parent;
+
+			if (nparent == ngrand_parent->left) {
+				if (ngrand_parent->right->color == RED) {
+					_set_color(nparent, BLACK);
+					_set_color(ngrand_parent->right, BLACK);
+					_set_color(ngrand_parent, RED);
+					node = ngrand_parent;
+					nparent = node->parent;
+				} else {
+					if (node == nparent->right) {
+						_rotate_left(nparent);
+						node = nparent;
+						nparent = node->parent;
+					}
+					_set_color(nparent, BLACK);
+					_set_color(ngrand_parent, RED);
+					_rotate_right(ngrand_parent);
+				}
+			} else {
+				if (ngrand_parent->left->color == RED) {
+					_set_color(nparent, BLACK);
+					_set_color(ngrand_parent->left, BLACK);
+					_set_color(ngrand_parent, RED);
+					node = ngrand_parent;
+					nparent = node->parent;
+				} else {
+					if (node == nparent->left) {
+						_rotate_right(nparent);
+						node = nparent;
+						nparent = node->parent;
+					}
+					_set_color(nparent, BLACK);
+					_set_color(ngrand_parent, RED);
+					_rotate_left(ngrand_parent);
+				}
+			}
+		}
+
+		_set_color(_data._root->left, BLACK);
+	}
+
+	Element *_insert(const T &p_value) {
+		Element *new_parent = _data._root;
+		Element *node = _data._root->left;
+		C less;
+
+		while (node != _data._nil) {
+			new_parent = node;
+
+			if (less(p_value, node->value)) {
+				node = node->left;
+			} else if (less(node->value, p_value)) {
+				node = node->right;
+			} else {
+				return node; // Return existing node
+			}
+		}
+
+		Element *new_node = memnew_allocator(Element, A);
+		new_node->parent = new_parent;
+		new_node->right = _data._nil;
+		new_node->left = _data._nil;
+		new_node->value = p_value;
+		//new_node->data=_data;
+
+		if (new_parent == _data._root || less(p_value, new_parent->value)) {
+			new_parent->left = new_node;
+		} else {
+			new_parent->right = new_node;
+		}
+
+		new_node->_next = _successor(new_node);
+		new_node->_prev = _predecessor(new_node);
+		if (new_node->_next) {
+			new_node->_next->_prev = new_node;
+		}
+		if (new_node->_prev) {
+			new_node->_prev->_next = new_node;
+		}
+
+		_data.size_cache++;
+		_insert_rb_fix(new_node);
+		return new_node;
+	}
+
+	void _erase_fix_rb(Element *p_node) {
+		Element *root = _data._root->left;
+		Element *node = _data._nil;
+		Element *sibling = p_node;
+		Element *parent = sibling->parent;
+
+		while (node != root) { // If red node found, will exit at a break
+			if (sibling->color == RED) {
+				_set_color(sibling, BLACK);
+				_set_color(parent, RED);
+				if (sibling == parent->right) {
+					sibling = sibling->left;
+					_rotate_left(parent);
+				} else {
+					sibling = sibling->right;
+					_rotate_right(parent);
+				}
+			}
+			if ((sibling->left->color == BLACK) && (sibling->right->color == BLACK)) {
+				_set_color(sibling, RED);
+				if (parent->color == RED) {
+					_set_color(parent, BLACK);
+					break;
+				} else { // loop: haven't found any red nodes yet
+					node = parent;
+					parent = node->parent;
+					sibling = (node == parent->left) ? parent->right : parent->left;
+				}
+			} else {
+				if (sibling == parent->right) {
+					if (sibling->right->color == BLACK) {
+						_set_color(sibling->left, BLACK);
+						_set_color(sibling, RED);
+						_rotate_right(sibling);
+						sibling = sibling->parent;
+					}
+					_set_color(sibling, parent->color);
+					_set_color(parent, BLACK);
+					_set_color(sibling->right, BLACK);
+					_rotate_left(parent);
+					break;
+				} else {
+					if (sibling->left->color == BLACK) {
+						_set_color(sibling->right, BLACK);
+						_set_color(sibling, RED);
+						_rotate_left(sibling);
+						sibling = sibling->parent;
+					}
+
+					_set_color(sibling, parent->color);
+					_set_color(parent, BLACK);
+					_set_color(sibling->left, BLACK);
+					_rotate_right(parent);
+					break;
+				}
+			}
+		}
+
+		ERR_FAIL_COND(_data._nil->color != BLACK);
+	}
+
+	void _erase(Element *p_node) {
+		Element *rp = ((p_node->left == _data._nil) || (p_node->right == _data._nil)) ? p_node : p_node->_next;
+		Element *node = (rp->left == _data._nil) ? rp->right : rp->left;
+
+		Element *sibling;
+		if (rp == rp->parent->left) {
+			rp->parent->left = node;
+			sibling = rp->parent->right;
+		} else {
+			rp->parent->right = node;
+			sibling = rp->parent->left;
+		}
+
+		if (node->color == RED) {
+			node->parent = rp->parent;
+			_set_color(node, BLACK);
+		} else if (rp->color == BLACK && rp->parent != _data._root) {
+			_erase_fix_rb(sibling);
+		}
+
+		if (rp != p_node) {
+			ERR_FAIL_COND(rp == _data._nil);
+
+			rp->left = p_node->left;
+			rp->right = p_node->right;
+			rp->parent = p_node->parent;
+			rp->color = p_node->color;
+			if (p_node->left != _data._nil) {
+				p_node->left->parent = rp;
+			}
+			if (p_node->right != _data._nil) {
+				p_node->right->parent = rp;
+			}
+
+			if (p_node == p_node->parent->left) {
+				p_node->parent->left = rp;
+			} else {
+				p_node->parent->right = rp;
+			}
+		}
+
+		if (p_node->_next) {
+			p_node->_next->_prev = p_node->_prev;
+		}
+		if (p_node->_prev) {
+			p_node->_prev->_next = p_node->_next;
+		}
+
+		memdelete_allocator<Element, A>(p_node);
+		_data.size_cache--;
+		ERR_FAIL_COND(_data._nil->color == RED);
+	}
+
+	void _calculate_depth(Element *p_element, int &max_d, int d) const {
+		if (p_element == _data._nil) {
+			return;
+		}
+
+		_calculate_depth(p_element->left, max_d, d + 1);
+		_calculate_depth(p_element->right, max_d, d + 1);
+
+		if (d > max_d) {
+			max_d = d;
+		}
+	}
+
+	void _cleanup_tree(Element *p_element) {
+		if (p_element == _data._nil) {
+			return;
+		}
+
+		_cleanup_tree(p_element->left);
+		_cleanup_tree(p_element->right);
+		memdelete_allocator<Element, A>(p_element);
+	}
+
+	void _copy_from(const RBSet &p_set) {
+		clear();
+		// not the fastest way, but safeset to write.
+		for (Element *I = p_set.front(); I; I = I->next()) {
+			insert(I->get());
+		}
+	}
+
+public:
+	const Element *find(const T &p_value) const {
+		if (!_data._root) {
+			return nullptr;
+		}
+
+		const Element *res = _find(p_value);
+		return res;
+	}
+
+	Element *find(const T &p_value) {
+		if (!_data._root) {
+			return nullptr;
+		}
+
+		Element *res = _find(p_value);
+		return res;
+	}
+
+	Element *lower_bound(const T &p_value) const {
+		if (!_data._root) {
+			return nullptr;
+		}
+		return _lower_bound(p_value);
+	}
+
+	bool has(const T &p_value) const {
+		return find(p_value) != nullptr;
+	}
+
+	Element *insert(const T &p_value) {
+		if (!_data._root) {
+			_data._create_root();
+		}
+		return _insert(p_value);
+	}
+
+	void erase(Element *p_element) {
+		if (!_data._root || !p_element) {
+			return;
+		}
+
+		_erase(p_element);
+		if (_data.size_cache == 0 && _data._root) {
+			_data._free_root();
+		}
+	}
+
+	bool erase(const T &p_value) {
+		if (!_data._root) {
+			return false;
+		}
+
+		Element *e = find(p_value);
+		if (!e) {
+			return false;
+		}
+
+		_erase(e);
+		if (_data.size_cache == 0 && _data._root) {
+			_data._free_root();
+		}
+		return true;
+	}
+
+	Element *front() const {
+		if (!_data._root) {
+			return nullptr;
+		}
+
+		Element *e = _data._root->left;
+		if (e == _data._nil) {
+			return nullptr;
+		}
+
+		while (e->left != _data._nil) {
+			e = e->left;
+		}
+
+		return e;
+	}
+
+	Element *back() const {
+		if (!_data._root) {
+			return nullptr;
+		}
+
+		Element *e = _data._root->left;
+		if (e == _data._nil) {
+			return nullptr;
+		}
+
+		while (e->right != _data._nil) {
+			e = e->right;
+		}
+
+		return e;
+	}
+
+	inline bool empty() const { return _data.size_cache == 0; }
+	inline int size() const { return _data.size_cache; }
+
+	int calculate_depth() const {
+		// used for debug mostly
+		if (!_data._root) {
+			return 0;
+		}
+
+		int max_d = 0;
+		_calculate_depth(_data._root->left, max_d, 0);
+		return max_d;
+	}
+
+	void clear() {
+		if (!_data._root) {
+			return;
+		}
+
+		_cleanup_tree(_data._root->left);
+		_data._root->left = _data._nil;
+		_data.size_cache = 0;
+		_data._free_root();
+	}
+
+	void operator=(const RBSet &p_set) {
+		_copy_from(p_set);
+	}
+
+	RBSet(const RBSet &p_set) {
+		_copy_from(p_set);
+	}
+
+	_FORCE_INLINE_ RBSet() {
+	}
+
+	~RBSet() {
+		clear();
+	}
+};
+
+#endif

core/containers/rid.h

@@ -0,0 +1,193 @@
+#ifndef RID_H
+#define RID_H
+/*************************************************************************/
+/*  rid.h                                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "core/containers/list.h"
+#include "core/os/memory.h"
+#include "core/containers/rid_handle.h"
+#include "core/os/safe_refcount.h"
+#include "core/containers/rb_set.h"
+#include "core/typedefs.h"
+
+#ifndef RID_HANDLES_ENABLED
+
+class RID_OwnerBase;
+
+class RID_Data {
+	friend class RID_OwnerBase;
+
+#ifndef DEBUG_ENABLED
+	RID_OwnerBase *_owner;
+#endif
+	uint32_t _id;
+
+public:
+	_FORCE_INLINE_ uint32_t get_id() const { return _id; }
+
+	virtual ~RID_Data();
+};
+
+class RID {
+	friend class RID_OwnerBase;
+
+	mutable RID_Data *_data;
+
+public:
+	_FORCE_INLINE_ RID_Data *get_data() const { return _data; }
+
+	_FORCE_INLINE_ bool operator==(const RID &p_rid) const {
+		return _data == p_rid._data;
+	}
+	_FORCE_INLINE_ bool operator<(const RID &p_rid) const {
+		return _data < p_rid._data;
+	}
+	_FORCE_INLINE_ bool operator<=(const RID &p_rid) const {
+		return _data <= p_rid._data;
+	}
+	_FORCE_INLINE_ bool operator>(const RID &p_rid) const {
+		return _data > p_rid._data;
+	}
+	_FORCE_INLINE_ bool operator!=(const RID &p_rid) const {
+		return _data != p_rid._data;
+	}
+	_FORCE_INLINE_ bool is_valid() const { return _data != nullptr; }
+
+	_FORCE_INLINE_ uint32_t get_id() const { return _data ? _data->get_id() : 0; }
+
+	_FORCE_INLINE_ RID() {
+		_data = nullptr;
+	}
+};
+
+class RID_OwnerBase {
+protected:
+	static SafeRefCount refcount;
+	_FORCE_INLINE_ void _set_data(RID &p_rid, RID_Data *p_data) {
+		p_rid._data = p_data;
+		p_data->_id = refcount.refval();
+#ifndef DEBUG_ENABLED
+		p_data->_owner = this;
+#endif
+	}
+
+#ifndef DEBUG_ENABLED
+
+	_FORCE_INLINE_ bool _is_owner(const RID &p_rid) const {
+		return this == p_rid._data->_owner;
+	}
+
+	_FORCE_INLINE_ void _remove_owner(RID &p_rid) {
+		p_rid._data->_owner = NULL;
+	}
+#endif
+
+public:
+	virtual void get_owned_list(List<RID> *p_owned) = 0;
+
+	static void init_rid();
+	virtual ~RID_OwnerBase() {}
+};
+
+template <class T>
+class RID_Owner : public RID_OwnerBase {
+public:
+#ifdef DEBUG_ENABLED
+	mutable RBSet<RID_Data *> id_map;
+#endif
+public:
+	_FORCE_INLINE_ RID make_rid(T *p_data) {
+		RID rid;
+		_set_data(rid, p_data);
+
+#ifdef DEBUG_ENABLED
+		id_map.insert(p_data);
+#endif
+
+		return rid;
+	}
+
+	_FORCE_INLINE_ T *get(const RID &p_rid) {
+#ifdef DEBUG_ENABLED
+
+		ERR_FAIL_COND_V(!p_rid.is_valid(), nullptr);
+		ERR_FAIL_COND_V(!id_map.has(p_rid.get_data()), nullptr);
+#endif
+		return static_cast<T *>(p_rid.get_data());
+	}
+
+	_FORCE_INLINE_ T *getornull(const RID &p_rid) {
+#ifdef DEBUG_ENABLED
+
+		if (p_rid.get_data()) {
+			ERR_FAIL_COND_V(!id_map.has(p_rid.get_data()), nullptr);
+		}
+#endif
+		return static_cast<T *>(p_rid.get_data());
+	}
+
+	_FORCE_INLINE_ T *getptr(const RID &p_rid) {
+		return static_cast<T *>(p_rid.get_data());
+	}
+
+	_FORCE_INLINE_ bool owns(const RID &p_rid) const {
+		if (p_rid.get_data() == nullptr) {
+			return false;
+		}
+#ifdef DEBUG_ENABLED
+		return id_map.has(p_rid.get_data());
+#else
+		return _is_owner(p_rid);
+#endif
+	}
+
+	void free(RID p_rid) {
+#ifdef DEBUG_ENABLED
+		id_map.erase(p_rid.get_data());
+#else
+		_remove_owner(p_rid);
+#endif
+	}
+
+	void get_owned_list(List<RID> *p_owned) {
+#ifdef DEBUG_ENABLED
+
+		for (typename RBSet<RID_Data *>::Element *E = id_map.front(); E; E = E->next()) {
+			RID r;
+			_set_data(r, static_cast<T *>(E->get()));
+			p_owned->push_back(r);
+		}
+#endif
+	}
+};
+
+#endif // not handles
+
+#endif

core/containers/rid_handle.h

@@ -0,0 +1,249 @@
+#ifndef RID_HANDLE_H
+#define RID_HANDLE_H
+/*************************************************************************/
+/*  rid_handle.h                                                         */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "core/containers/list.h"
+#include "core/os/mutex.h"
+#include "core/containers/pooled_list.h"
+#include "core/os/safe_refcount.h"
+#include "core/typedefs.h"
+
+#include <typeinfo>
+
+// SCONS parameters:
+// rids=pointers (default)
+// rids=handles
+// rids=tracked_handles (handles plus allocation tracking)
+
+// Defines RID_HANDLES_ENABLED and RID_HANDLE_ALLOCATION_TRACKING_ENABLED
+// should be defined from Scons as required following the above convention.
+
+// RID_PRIME is the macro which stores line numbers and file in the RID_Database.
+// It will be a NOOP if tracking is off.
+#ifdef RID_HANDLE_ALLOCATION_TRACKING_ENABLED
+#define RID_PRIME(a) g_rid_database.prime(a, __LINE__, __FILE__)
+#else
+#define RID_PRIME(a) a
+#endif
+
+// All the handle code can be compiled out if they are not in use.
+#ifdef RID_HANDLES_ENABLED
+
+// This define will print out each make_rid and free_rid. Useful for debugging.
+// #define RID_HANDLE_PRINT_LIFETIMES
+
+class RID_OwnerBase;
+class RID_Database;
+
+class RID_Data {
+	friend class RID_OwnerBase;
+	friend class RID_Database;
+
+	RID_OwnerBase *_owner;
+	uint32_t _id;
+
+public:
+	uint32_t get_id() const { return _id; }
+
+	virtual ~RID_Data();
+};
+
+class RID_Handle {
+public:
+	union {
+		struct {
+			uint32_t _id;
+			uint32_t _revision;
+		};
+		uint64_t _handle_data;
+	};
+
+	RID_Handle() {
+		_handle_data = 0;
+	}
+
+	bool operator==(const RID_Handle &p_rid) const {
+		return _handle_data == p_rid._handle_data;
+	}
+	bool operator<(const RID_Handle &p_rid) const {
+		return _handle_data < p_rid._handle_data;
+	}
+	bool operator<=(const RID_Handle &p_rid) const {
+		return _handle_data <= p_rid._handle_data;
+	}
+	bool operator>(const RID_Handle &p_rid) const {
+		return _handle_data > p_rid._handle_data;
+	}
+	bool operator!=(const RID_Handle &p_rid) const {
+		return _handle_data != p_rid._handle_data;
+	}
+	bool is_valid() const { return _id != 0; }
+
+	uint32_t get_id() const { return _id ? _handle_data : 0; }
+};
+
+class RID : public RID_Handle {
+};
+
+class RID_Database {
+	struct PoolElement {
+		RID_Data *data;
+		uint32_t revision;
+#ifdef RID_HANDLE_ALLOCATION_TRACKING_ENABLED
+		// current allocation
+		uint16_t line_number;
+		uint16_t owner_name_id;
+		const char *filename;
+
+		// previous allocation (allows identifying dangling RID source allocations)
+		const char *previous_filename;
+		uint32_t previous_line_number;
+#endif
+	};
+
+	struct Leak {
+		uint16_t line_number;
+		uint16_t owner_name_id;
+		const char *filename;
+		uint32_t num_objects_leaked;
+	};
+
+	// The pooled list zeros on first request .. this is important
+	// so that we initialize the revision to zero. Other than that, it
+	// is treated as a POD type.
+	TrackedPooledList<PoolElement, uint32_t, true, true> _pool;
+	bool _shutdown = false;
+	mutable Mutex _mutex;
+
+	// This is purely for printing the leaks at the end, as RID_Owners may be
+	// destroyed before the RID_Database is shutdown, so the RID_Data may be invalid
+	// by this point, and we still want to have a record of the owner names.
+	// The owner names should part of the binary, thus the pointers should still be valid.
+	// They were retrieved using typeid(T).name()
+	LocalVector<const char *> _owner_names;
+	LocalVector<Leak> _leaks;
+
+	void register_leak(uint32_t p_line_number, uint32_t p_owner_name_id, const char *p_filename);
+	String _rid_to_string(const RID &p_rid, const PoolElement &p_pe) const;
+
+public:
+	RID_Database();
+	~RID_Database();
+
+	// Called to record the owner names before RID_Owners are destroyed
+	void preshutdown();
+
+	// Called after destroying RID_Owners to detect leaks
+	void shutdown();
+
+	// Prepare a RID for memory tracking
+	RID prime(const RID &p_rid, int p_line_number, const char *p_filename);
+
+	void handle_make_rid(RID &r_rid, RID_Data *p_data, RID_OwnerBase *p_owner);
+	RID_Data *handle_get(const RID &p_rid) const;
+	RID_Data *handle_getptr(const RID &p_rid) const;
+	RID_Data *handle_get_or_null(const RID &p_rid) const;
+
+	bool handle_is_owner(const RID &p_rid, const RID_OwnerBase *p_owner) const;
+	void handle_free(const RID &p_rid);
+};
+
+extern RID_Database g_rid_database;
+
+class RID_OwnerBase {
+protected:
+	bool _is_owner(const RID &p_rid) const {
+		return g_rid_database.handle_is_owner(p_rid, this);
+	}
+
+	void _rid_print(const char *pszType, String sz, const RID &p_rid);
+
+	const char *_typename = nullptr;
+	bool _shutdown = false;
+
+public:
+	virtual void get_owned_list(List<RID> *p_owned) = 0;
+	const char *get_typename() const { return _typename; }
+
+	static void init_rid();
+	virtual ~RID_OwnerBase();
+};
+
+template <class T>
+class RID_Owner : public RID_OwnerBase {
+public:
+	RID make_rid(T *p_data) {
+		RID rid;
+		g_rid_database.handle_make_rid(rid, p_data, this);
+
+#ifdef RID_HANDLE_PRINT_LIFETIMES
+		_rid_print(_typename, "make_rid", rid);
+#endif
+		return rid;
+	}
+
+	T *get(const RID &p_rid) {
+		return static_cast<T *>(g_rid_database.handle_get(p_rid));
+	}
+
+	T *getornull(const RID &p_rid) {
+		return static_cast<T *>(g_rid_database.handle_get_or_null(p_rid));
+	}
+
+	T *getptr(const RID &p_rid) {
+		return static_cast<T *>(g_rid_database.handle_getptr(p_rid));
+	}
+
+	bool owns(const RID &p_rid) const {
+		return _is_owner(p_rid);
+	}
+
+	void free(RID p_rid) {
+#ifdef RID_HANDLE_PRINT_LIFETIMES
+		_rid_print(_typename, "free_rid", p_rid);
+#endif
+		g_rid_database.handle_free(p_rid);
+	}
+
+	void get_owned_list(List<RID> *p_owned){
+#ifdef DEBUG_ENABLED
+
+#endif
+	}
+
+	RID_Owner() {
+		_typename = typeid(T).name();
+	}
+};
+
+#endif // RID_HANDLES_ENABLED
+
+#endif // RID_HANDLE_H

core/containers/ring_buffer.h

@@ -0,0 +1,221 @@
+#ifndef RINGBUFFER_H
+#define RINGBUFFER_H
+/*************************************************************************/
+/*  ring_buffer.h                                                        */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "core/containers/vector.h"
+
+template <typename T>
+class RingBuffer {
+	Vector<T> data;
+	int read_pos;
+	int write_pos;
+	int size_mask;
+
+	inline int inc(int &p_var, int p_size) const {
+		int ret = p_var;
+		p_var += p_size;
+		p_var = p_var & size_mask;
+		return ret;
+	};
+
+public:
+	T read() {
+		ERR_FAIL_COND_V(space_left() < 1, T());
+		return data.ptr()[inc(read_pos, 1)];
+	};
+
+	int read(T *p_buf, int p_size, bool p_advance = true) {
+		int left = data_left();
+		p_size = MIN(left, p_size);
+		int pos = read_pos;
+		int to_read = p_size;
+		int dst = 0;
+		while (to_read) {
+			int end = pos + to_read;
+			end = MIN(end, size());
+			int total = end - pos;
+			const T *read = data.ptr();
+			for (int i = 0; i < total; i++) {
+				p_buf[dst++] = read[pos + i];
+			};
+			to_read -= total;
+			pos = 0;
+		};
+		if (p_advance) {
+			inc(read_pos, p_size);
+		};
+		return p_size;
+	};
+
+	int copy(T *p_buf, int p_offset, int p_size) const {
+		int left = data_left();
+		if ((p_offset + p_size) > left) {
+			p_size -= left - p_offset;
+			if (p_size <= 0) {
+				return 0;
+			}
+		}
+		p_size = MIN(left, p_size);
+		int pos = read_pos;
+		inc(pos, p_offset);
+		int to_read = p_size;
+		int dst = 0;
+		while (to_read) {
+			int end = pos + to_read;
+			end = MIN(end, size());
+			int total = end - pos;
+			for (int i = 0; i < total; i++) {
+				p_buf[dst++] = data[pos + i];
+			};
+			to_read -= total;
+			pos = 0;
+		};
+		return p_size;
+	};
+
+	int find(const T &t, int p_offset, int p_max_size) const {
+		int left = data_left();
+		if ((p_offset + p_max_size) > left) {
+			p_max_size -= left - p_offset;
+			if (p_max_size <= 0) {
+				return 0;
+			}
+		}
+		p_max_size = MIN(left, p_max_size);
+		int pos = read_pos;
+		inc(pos, p_offset);
+		int to_read = p_max_size;
+		while (to_read) {
+			int end = pos + to_read;
+			end = MIN(end, size());
+			int total = end - pos;
+			for (int i = 0; i < total; i++) {
+				if (data[pos + i] == t) {
+					return i + (p_max_size - to_read);
+				}
+			};
+			to_read -= total;
+			pos = 0;
+		}
+		return -1;
+	}
+
+	inline int advance_read(int p_n) {
+		p_n = MIN(p_n, data_left());
+		inc(read_pos, p_n);
+		return p_n;
+	};
+
+	inline int decrease_write(int p_n) {
+		p_n = MIN(p_n, data_left());
+		inc(write_pos, size_mask + 1 - p_n);
+		return p_n;
+	}
+
+	Error write(const T &p_v) {
+		ERR_FAIL_COND_V(space_left() < 1, FAILED);
+		data.write[inc(write_pos, 1)] = p_v;
+		return OK;
+	};
+
+	int write(const T *p_buf, int p_size) {
+		int left = space_left();
+		p_size = MIN(left, p_size);
+
+		int pos = write_pos;
+		int to_write = p_size;
+		int src = 0;
+		while (to_write) {
+			int end = pos + to_write;
+			end = MIN(end, size());
+			int total = end - pos;
+
+			for (int i = 0; i < total; i++) {
+				data.write[pos + i] = p_buf[src++];
+			};
+			to_write -= total;
+			pos = 0;
+		};
+
+		inc(write_pos, p_size);
+		return p_size;
+	};
+
+	inline int space_left() const {
+		int left = read_pos - write_pos;
+		if (left < 0) {
+			return size() + left - 1;
+		};
+		if (left == 0) {
+			return size() - 1;
+		};
+		return left - 1;
+	};
+	inline int data_left() const {
+		return size() - space_left() - 1;
+	};
+
+	inline int size() const {
+		return data.size();
+	};
+
+	inline void clear() {
+		read_pos = 0;
+		write_pos = 0;
+	}
+
+	void resize(int p_power) {
+		int old_size = size();
+		int new_size = 1 << p_power;
+		int mask = new_size - 1;
+		data.resize(1 << p_power);
+		if (old_size < new_size && read_pos > write_pos) {
+			for (int i = 0; i < write_pos; i++) {
+				data.write[(old_size + i) & mask] = data[i];
+			};
+			write_pos = (old_size + write_pos) & mask;
+		} else {
+			read_pos = read_pos & mask;
+			write_pos = write_pos & mask;
+		};
+
+		size_mask = mask;
+	};
+
+	RingBuffer<T>(int p_power = 0) {
+		read_pos = 0;
+		write_pos = 0;
+		resize(p_power);
+	};
+	~RingBuffer<T>(){};
+};
+
+#endif

core/containers/safe_list.h

@@ -0,0 +1,258 @@
+/**************************************************************************/
+/*  safe_list.h                                                           */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef SAFE_LIST_H
+#define SAFE_LIST_H
+
+#include "core/os/memory.h"
+#include "core/typedefs.h"
+
+#include <atomic>
+#include <functional>
+#include <type_traits>
+
+// Design goals for these classes:
+// - Accessing this list with an iterator will never result in a use-after free,
+//   even if the element being accessed has been logically removed from the list on
+//   another thread.
+// - Logical deletion from the list will not result in deallocation at that time,
+//   instead the node will be deallocated at a later time when it is safe to do so.
+// - No blocking synchronization primitives will be used.
+
+// This is used in very specific areas of the engine where it's critical that these guarantees are held.
+
+template <class T, class A = DefaultAllocator>
+class SafeList {
+	struct SafeListNode {
+		std::atomic<SafeListNode *> next = nullptr;
+
+		// If the node is logically deleted, this pointer will typically point
+		// to the previous list item in time that was also logically deleted.
+		std::atomic<SafeListNode *> graveyard_next = nullptr;
+
+		void (*deletion_fn)(T t);
+
+		T val;
+
+		static void default_deletion_fn(T) {}
+
+		SafeListNode() {
+			next = NULL;
+			graveyard_next = NULL;
+			deletion_fn = default_deletion_fn;
+		}
+	};
+
+	std::atomic<SafeListNode *> head;
+	std::atomic<SafeListNode *> graveyard_head;
+
+	std::atomic<uint32_t> active_iterator_count;
+
+public:
+	class Iterator {
+		friend class SafeList;
+
+		SafeListNode *cursor;
+		SafeList *list;
+
+		Iterator(SafeListNode *p_cursor, SafeList *p_list) {
+			cursor = p_cursor;
+			list = p_list;
+			list->active_iterator_count++;
+		}
+
+	public:
+		Iterator(const Iterator &p_other) {
+			cursor = p_other.cursor;
+			list = p_other.list;
+			list->active_iterator_count++;
+		}
+
+		~Iterator() {
+			list->active_iterator_count--;
+		}
+
+	public:
+		T &operator*() {
+			return cursor->val;
+		}
+
+		Iterator &operator++() {
+			cursor = cursor->next;
+			return *this;
+		}
+
+		// These two operators are mostly useful for comparisons to nullptr.
+		bool operator==(const void *p_other) const {
+			return cursor == p_other;
+		}
+
+		bool operator!=(const void *p_other) const {
+			return cursor != p_other;
+		}
+
+		// These two allow easy range-based for loops.
+		bool operator==(const Iterator &p_other) const {
+			return cursor == p_other.cursor;
+		}
+
+		bool operator!=(const Iterator &p_other) const {
+			return cursor != p_other.cursor;
+		}
+	};
+
+public:
+	// Calling this will cause an allocation.
+	void insert(T p_value) {
+		SafeListNode *new_node = memnew_allocator(SafeListNode, A);
+		new_node->val = p_value;
+		SafeListNode *expected_head = nullptr;
+		do {
+			expected_head = head.load();
+			new_node->next.store(expected_head);
+		} while (!head.compare_exchange_strong(/* expected= */ expected_head, /* new= */ new_node));
+	}
+
+	Iterator find(T p_value) {
+		for (Iterator it = begin(); it != end(); ++it) {
+			if (*it == p_value) {
+				return it;
+			}
+		}
+		return end();
+	}
+
+	void erase(T p_value, std::function<void(T)> p_deletion_fn) {
+		Iterator tmp = find(p_value);
+		erase(tmp, p_deletion_fn);
+	}
+
+	void erase(T p_value) {
+		Iterator tmp = find(p_value);
+		erase(tmp, [](T t) { return; });
+	}
+
+	void erase(Iterator &p_iterator, std::function<void(T)> p_deletion_fn) {
+		p_iterator.cursor->deletion_fn = p_deletion_fn;
+		erase(p_iterator);
+	}
+
+	void erase(Iterator &p_iterator) {
+		if (find(p_iterator.cursor->val) == nullptr) {
+			// Not in the list, nothing to do.
+			return;
+		}
+		// First, remove the node from the list.
+		while (true) {
+			Iterator prev = begin();
+			SafeListNode *expected_head = prev.cursor;
+			for (; prev != end(); ++prev) {
+				if (prev.cursor && prev.cursor->next == p_iterator.cursor) {
+					break;
+				}
+			}
+			if (prev != end()) {
+				// There exists a node before this.
+				prev.cursor->next.store(p_iterator.cursor->next.load());
+				// Done.
+				break;
+			} else {
+				if (head.compare_exchange_strong(/* expected= */ expected_head, /* new= */ p_iterator.cursor->next.load())) {
+					// Successfully reassigned the head pointer before another thread changed it to something else.
+					break;
+				}
+				// Fall through upon failure, try again.
+			}
+		}
+		// Then queue it for deletion by putting it in the node graveyard.
+		// Don't touch `next` because an iterator might still be pointing at this node.
+		SafeListNode *expected_head = nullptr;
+		do {
+			expected_head = graveyard_head.load();
+			p_iterator.cursor->graveyard_next.store(expected_head);
+		} while (!graveyard_head.compare_exchange_strong(/* expected= */ expected_head, /* new= */ p_iterator.cursor));
+	}
+
+	Iterator begin() {
+		return Iterator(head.load(), this);
+	}
+
+	Iterator end() {
+		return Iterator(NULL, this);
+	}
+
+	// Calling this will cause zero to many deallocations.
+	bool maybe_cleanup() {
+		SafeListNode *cursor = NULL;
+		SafeListNode *new_graveyard_head = NULL;
+
+		do {
+			// The access order here is theoretically important.
+			cursor = graveyard_head.load();
+			if (active_iterator_count.load() != 0) {
+				// It's not safe to clean up with an active iterator, because that iterator
+				// could be pointing to an element that we want to delete.
+				return false;
+			}
+			// Any iterator created after this point will never point to a deleted node.
+			// Swap it out with the current graveyard head.
+		} while (!graveyard_head.compare_exchange_strong(/* expected= */ cursor, /* new= */ new_graveyard_head));
+
+		// Our graveyard list is now unreachable by any active iterators,
+		// detached from the main graveyard head and ready for deletion.
+		while (cursor) {
+			SafeListNode *tmp = cursor;
+			cursor = cursor->graveyard_next;
+			tmp->deletion_fn(tmp->val);
+			memdelete_allocator<SafeListNode, A>(tmp);
+		}
+
+		return true;
+	}
+
+	SafeList() {
+		head = NULL;
+		graveyard_head = NULL;
+
+		active_iterator_count = 0;
+	}
+
+	~SafeList() {
+#ifdef DEBUG_ENABLED
+		if (!maybe_cleanup()) {
+			ERR_PRINT("There are still iterators around when destructing a SafeList. Memory will be leaked. This is a bug.");
+		}
+#else
+		maybe_cleanup();
+#endif
+	}
+};
+
+#endif // SAFE_LIST_H

core/containers/search_array.h

@@ -0,0 +1,67 @@
+/**************************************************************************/
+/*  search_array.h                                                        */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef SEARCH_ARRAY_H
+#define SEARCH_ARRAY_H
+
+#include "sort_array.h"
+
+template <class T, class Comparator = _DefaultComparator<T>>
+class SearchArray {
+public:
+	Comparator compare;
+
+	inline int bisect(const T *p_array, int p_len, const T &p_value, bool p_before) const {
+		int lo = 0;
+		int hi = p_len;
+		if (p_before) {
+			while (lo < hi) {
+				const int mid = (lo + hi) / 2;
+				if (compare(p_array[mid], p_value)) {
+					lo = mid + 1;
+				} else {
+					hi = mid;
+				}
+			}
+		} else {
+			while (lo < hi) {
+				const int mid = (lo + hi) / 2;
+				if (compare(p_value, p_array[mid])) {
+					hi = mid;
+				} else {
+					lo = mid + 1;
+				}
+			}
+		}
+		return lo;
+	}
+};
+
+#endif // SEARCH_ARRAY_H

core/containers/self_list.h

@@ -0,0 +1,143 @@
+#ifndef SELF_LIST_H
+#define SELF_LIST_H
+/*************************************************************************/
+/*  self_list.h                                                          */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "core/error/error_macros.h"
+#include "core/typedefs.h"
+
+template <class T>
+class SelfList {
+public:
+	class List {
+		SelfList<T> *_first;
+		SelfList<T> *_last;
+
+	public:
+		void add(SelfList<T> *p_elem) {
+			ERR_FAIL_COND(p_elem->_root);
+
+			p_elem->_root = this;
+			p_elem->_next = _first;
+			p_elem->_prev = nullptr;
+
+			if (_first) {
+				_first->_prev = p_elem;
+
+			} else {
+				_last = p_elem;
+			}
+
+			_first = p_elem;
+		}
+
+		void add_last(SelfList<T> *p_elem) {
+			ERR_FAIL_COND(p_elem->_root);
+
+			p_elem->_root = this;
+			p_elem->_next = nullptr;
+			p_elem->_prev = _last;
+
+			if (_last) {
+				_last->_next = p_elem;
+
+			} else {
+				_first = p_elem;
+			}
+
+			_last = p_elem;
+		}
+
+		void remove(SelfList<T> *p_elem) {
+			ERR_FAIL_COND(p_elem->_root != this);
+			if (p_elem->_next) {
+				p_elem->_next->_prev = p_elem->_prev;
+			}
+
+			if (p_elem->_prev) {
+				p_elem->_prev->_next = p_elem->_next;
+			}
+
+			if (_first == p_elem) {
+				_first = p_elem->_next;
+			}
+
+			if (_last == p_elem) {
+				_last = p_elem->_prev;
+			}
+
+			p_elem->_next = nullptr;
+			p_elem->_prev = nullptr;
+			p_elem->_root = nullptr;
+		}
+
+		_FORCE_INLINE_ SelfList<T> *first() { return _first; }
+		_FORCE_INLINE_ const SelfList<T> *first() const { return _first; }
+		_FORCE_INLINE_ List() {
+			_first = nullptr;
+			_last = nullptr;
+		}
+		_FORCE_INLINE_ ~List() { ERR_FAIL_COND(_first != nullptr); }
+	};
+
+private:
+	List *_root;
+	T *_self;
+	SelfList<T> *_next;
+	SelfList<T> *_prev;
+
+public:
+	_FORCE_INLINE_ bool in_list() const { return _root; }
+	_FORCE_INLINE_ void remove_from_list() {
+		if (_root) {
+			_root->remove(this);
+		}
+	}
+	_FORCE_INLINE_ SelfList<T> *next() { return _next; }
+	_FORCE_INLINE_ SelfList<T> *prev() { return _prev; }
+	_FORCE_INLINE_ const SelfList<T> *next() const { return _next; }
+	_FORCE_INLINE_ const SelfList<T> *prev() const { return _prev; }
+	_FORCE_INLINE_ T *self() const { return _self; }
+
+	_FORCE_INLINE_ SelfList(T *p_self) {
+		_self = p_self;
+		_next = nullptr;
+		_prev = nullptr;
+		_root = nullptr;
+	}
+
+	_FORCE_INLINE_ ~SelfList() {
+		if (_root) {
+			_root->remove(this);
+		}
+	}
+};
+
+#endif // SELF_LIST_H

core/containers/simple_type.h

@@ -0,0 +1,50 @@
+#ifndef SIMPLE_TYPE_H
+#define SIMPLE_TYPE_H
+/*************************************************************************/
+/*  simple_type.h                                                        */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+/* Batch of specializations to obtain the actual simple type */
+
+template <class T>
+struct GetSimpleTypeT {
+	typedef T type_t;
+};
+
+template <class T>
+struct GetSimpleTypeT<T &> {
+	typedef T type_t;
+};
+
+template <class T>
+struct GetSimpleTypeT<T const> {
+	typedef T type_t;
+};
+
+#endif

core/containers/sort_array.h

@@ -0,0 +1,320 @@
+#ifndef SORT_ARRAY_H
+#define SORT_ARRAY_H
+/*************************************************************************/
+/*  sort_array.h                                                         */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "core/error/error_macros.h"
+#include "core/typedefs.h"
+
+#define ERR_BAD_COMPARE(cond)                                         \
+	if (unlikely(cond)) {                                             \
+		ERR_PRINT("bad comparison function; sorting will be broken"); \
+		break;                                                        \
+	}
+
+template <class T>
+struct _DefaultComparator {
+	_FORCE_INLINE_ bool operator()(const T &a, const T &b) const { return (a < b); }
+};
+
+#ifdef DEBUG_ENABLED
+#define SORT_ARRAY_VALIDATE_ENABLED true
+#else
+#define SORT_ARRAY_VALIDATE_ENABLED false
+#endif
+
+template <class T, class Comparator = _DefaultComparator<T>, bool Validate = SORT_ARRAY_VALIDATE_ENABLED>
+class SortArray {
+	enum {
+
+		INTROSORT_THRESHOLD = 16
+	};
+
+public:
+	Comparator compare;
+
+	inline const T &median_of_3(const T &a, const T &b, const T &c) const {
+		if (compare(a, b)) {
+			if (compare(b, c)) {
+				return b;
+			} else if (compare(a, c)) {
+				return c;
+			} else {
+				return a;
+			}
+		} else if (compare(a, c)) {
+			return a;
+		} else if (compare(b, c)) {
+			return c;
+		} else {
+			return b;
+		}
+	}
+
+	inline int bitlog(int n) const {
+		int k;
+		for (k = 0; n != 1; n >>= 1) {
+			++k;
+		}
+		return k;
+	}
+
+	/* Heap / Heapsort functions */
+
+	inline void push_heap(int p_first, int p_hole_idx, int p_top_index, T p_value, T *p_array) const {
+		int parent = (p_hole_idx - 1) / 2;
+		while (p_hole_idx > p_top_index && compare(p_array[p_first + parent], p_value)) {
+			p_array[p_first + p_hole_idx] = p_array[p_first + parent];
+			p_hole_idx = parent;
+			parent = (p_hole_idx - 1) / 2;
+		}
+		p_array[p_first + p_hole_idx] = p_value;
+	}
+
+	inline void pop_heap(int p_first, int p_last, int p_result, T p_value, T *p_array) const {
+		p_array[p_result] = p_array[p_first];
+		adjust_heap(p_first, 0, p_last - p_first, p_value, p_array);
+	}
+	inline void pop_heap(int p_first, int p_last, T *p_array) const {
+		pop_heap(p_first, p_last - 1, p_last - 1, p_array[p_last - 1], p_array);
+	}
+
+	inline void adjust_heap(int p_first, int p_hole_idx, int p_len, T p_value, T *p_array) const {
+		int top_index = p_hole_idx;
+		int second_child = 2 * p_hole_idx + 2;
+
+		while (second_child < p_len) {
+			if (compare(p_array[p_first + second_child], p_array[p_first + (second_child - 1)])) {
+				second_child--;
+			}
+
+			p_array[p_first + p_hole_idx] = p_array[p_first + second_child];
+			p_hole_idx = second_child;
+			second_child = 2 * (second_child + 1);
+		}
+
+		if (second_child == p_len) {
+			p_array[p_first + p_hole_idx] = p_array[p_first + (second_child - 1)];
+			p_hole_idx = second_child - 1;
+		}
+		push_heap(p_first, p_hole_idx, top_index, p_value, p_array);
+	}
+
+	inline void sort_heap(int p_first, int p_last, T *p_array) const {
+		while (p_last - p_first > 1) {
+			pop_heap(p_first, p_last--, p_array);
+		}
+	}
+
+	inline void make_heap(int p_first, int p_last, T *p_array) const {
+		if (p_last - p_first < 2) {
+			return;
+		}
+		int len = p_last - p_first;
+		int parent = (len - 2) / 2;
+
+		while (true) {
+			adjust_heap(p_first, parent, len, p_array[p_first + parent], p_array);
+			if (parent == 0) {
+				return;
+			}
+			parent--;
+		}
+	}
+
+	inline void partial_sort(int p_first, int p_last, int p_middle, T *p_array) const {
+		make_heap(p_first, p_middle, p_array);
+		for (int i = p_middle; i < p_last; i++) {
+			if (compare(p_array[i], p_array[p_first])) {
+				pop_heap(p_first, p_middle, i, p_array[i], p_array);
+			}
+		}
+		sort_heap(p_first, p_middle, p_array);
+	}
+
+	inline void partial_select(int p_first, int p_last, int p_middle, T *p_array) const {
+		make_heap(p_first, p_middle, p_array);
+		for (int i = p_middle; i < p_last; i++) {
+			if (compare(p_array[i], p_array[p_first])) {
+				pop_heap(p_first, p_middle, i, p_array[i], p_array);
+			}
+		}
+	}
+
+	inline int partitioner(int p_first, int p_last, T p_pivot, T *p_array) const {
+		const int unmodified_first = p_first;
+		const int unmodified_last = p_last;
+
+		while (true) {
+			while (compare(p_array[p_first], p_pivot)) {
+				if (Validate) {
+					ERR_BAD_COMPARE(p_first == unmodified_last - 1);
+				}
+				p_first++;
+			}
+			p_last--;
+			while (compare(p_pivot, p_array[p_last])) {
+				if (Validate) {
+					ERR_BAD_COMPARE(p_last == unmodified_first);
+				}
+				p_last--;
+			}
+
+			if (!(p_first < p_last)) {
+				return p_first;
+			}
+
+			SWAP(p_array[p_first], p_array[p_last]);
+			p_first++;
+		}
+	}
+
+	inline void introsort(int p_first, int p_last, T *p_array, int p_max_depth) const {
+		while (p_last - p_first > INTROSORT_THRESHOLD) {
+			if (p_max_depth == 0) {
+				partial_sort(p_first, p_last, p_last, p_array);
+				return;
+			}
+
+			p_max_depth--;
+
+			int cut = partitioner(
+					p_first,
+					p_last,
+					median_of_3(
+							p_array[p_first],
+							p_array[p_first + (p_last - p_first) / 2],
+							p_array[p_last - 1]),
+					p_array);
+
+			introsort(cut, p_last, p_array, p_max_depth);
+			p_last = cut;
+		}
+	}
+
+	inline void introselect(int p_first, int p_nth, int p_last, T *p_array, int p_max_depth) const {
+		while (p_last - p_first > 3) {
+			if (p_max_depth == 0) {
+				partial_select(p_first, p_nth + 1, p_last, p_array);
+				SWAP(p_first, p_nth);
+				return;
+			}
+
+			p_max_depth--;
+
+			int cut = partitioner(
+					p_first,
+					p_last,
+					median_of_3(
+							p_array[p_first],
+							p_array[p_first + (p_last - p_first) / 2],
+							p_array[p_last - 1]),
+					p_array);
+
+			if (cut <= p_nth) {
+				p_first = cut;
+			} else {
+				p_last = cut;
+			}
+		}
+
+		insertion_sort(p_first, p_last, p_array);
+	}
+
+	inline void unguarded_linear_insert(int p_last, T p_value, T *p_array) const {
+		int next = p_last - 1;
+		while (compare(p_value, p_array[next])) {
+			if (Validate) {
+				ERR_BAD_COMPARE(next == 0);
+			}
+			p_array[p_last] = p_array[next];
+			p_last = next;
+			next--;
+		}
+		p_array[p_last] = p_value;
+	}
+
+	inline void linear_insert(int p_first, int p_last, T *p_array) const {
+		T val = p_array[p_last];
+		if (compare(val, p_array[p_first])) {
+			for (int i = p_last; i > p_first; i--) {
+				p_array[i] = p_array[i - 1];
+			}
+
+			p_array[p_first] = val;
+		} else {
+			unguarded_linear_insert(p_last, val, p_array);
+		}
+	}
+
+	inline void insertion_sort(int p_first, int p_last, T *p_array) const {
+		if (p_first == p_last) {
+			return;
+		}
+		for (int i = p_first + 1; i != p_last; i++) {
+			linear_insert(p_first, i, p_array);
+		}
+	}
+
+	inline void unguarded_insertion_sort(int p_first, int p_last, T *p_array) const {
+		for (int i = p_first; i != p_last; i++) {
+			unguarded_linear_insert(i, p_array[i], p_array);
+		}
+	}
+
+	inline void final_insertion_sort(int p_first, int p_last, T *p_array) const {
+		if (p_last - p_first > INTROSORT_THRESHOLD) {
+			insertion_sort(p_first, p_first + INTROSORT_THRESHOLD, p_array);
+			unguarded_insertion_sort(p_first + INTROSORT_THRESHOLD, p_last, p_array);
+		} else {
+			insertion_sort(p_first, p_last, p_array);
+		}
+	}
+
+	inline void sort_range(int p_first, int p_last, T *p_array) const {
+		if (p_first != p_last) {
+			introsort(p_first, p_last, p_array, bitlog(p_last - p_first) * 2);
+			final_insertion_sort(p_first, p_last, p_array);
+		}
+	}
+
+	inline void sort(T *p_array, int p_len) const {
+		sort_range(0, p_len, p_array);
+	}
+
+	inline void nth_element(int p_first, int p_last, int p_nth, T *p_array) const {
+		if (p_first == p_last || p_nth == p_last) {
+			return;
+		}
+		introselect(p_first, p_nth, p_last, p_array, bitlog(p_last - p_first) * 2);
+	}
+};
+
+#endif // SORT_ARRAY_H

core/containers/threaded_callable_queue.h

@@ -0,0 +1,132 @@
+#ifndef THREADED_CALLABLE_QUEUE_H
+#define THREADED_CALLABLE_QUEUE_H
+/*************************************************************************/
+/*  threaded_callable_queue.h                                            */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "core/containers/local_vector.h"
+#include "core/containers/ordered_hash_map.h"
+#include "core/os/mutex.h"
+#include "core/os/semaphore.h"
+#include "core/os/thread.h"
+
+#include <functional>
+
+template <class K>
+class ThreadedCallableQueue {
+public:
+	using Job = std::function<void()>;
+
+private:
+	bool exit;
+	Thread thread;
+	BinaryMutex mutex;
+	Semaphore sem;
+	OrderedHashMap<K, Job> queue;
+
+	static void _thread_func(void *p_user_data);
+
+public:
+	void enqueue(K p_key, Job p_job);
+	void cancel(K p_key);
+
+	ThreadedCallableQueue();
+	~ThreadedCallableQueue();
+};
+
+template <class K>
+void ThreadedCallableQueue<K>::_thread_func(void *p_user_data) {
+	ThreadedCallableQueue *self = static_cast<ThreadedCallableQueue *>(p_user_data);
+
+	while (true) {
+		self->sem.wait();
+		self->mutex.lock();
+		if (self->exit) {
+			self->mutex.unlock();
+			break;
+		}
+
+		typename OrderedHashMap<K, Job>::Element E = self->queue.front();
+		// Defense about implementation bugs (excessive posts)
+		if (!E) {
+			ERR_PRINT("Semaphore unlocked, the queue is empty. Bug?");
+			self->mutex.unlock();
+			// --- Defense end
+		} else {
+			LocalVector<Job> jobs;
+			jobs.push_back(E.value());
+			self->queue.erase(E);
+			self->mutex.unlock();
+
+			for (uint32_t i = 0; i < jobs.size(); i++) {
+				jobs[i]();
+			}
+		}
+	}
+
+	self->mutex.lock();
+	for (typename OrderedHashMap<K, Job>::Element E = self->queue.front(); E; E = E.next()) {
+		Job job = E.value();
+		job();
+	}
+	self->mutex.unlock();
+}
+
+template <class K>
+void ThreadedCallableQueue<K>::enqueue(K p_key, Job p_job) {
+	MutexLock lock(mutex);
+	ERR_FAIL_COND(exit);
+	ERR_FAIL_COND(queue.has(p_key));
+	queue.insert(p_key, p_job);
+	sem.post();
+}
+
+template <class K>
+void ThreadedCallableQueue<K>::cancel(K p_key) {
+	MutexLock lock(mutex);
+	ERR_FAIL_COND(exit);
+	if (queue.erase(p_key)) {
+		sem.wait();
+	}
+}
+
+template <class K>
+ThreadedCallableQueue<K>::ThreadedCallableQueue() :
+		exit(false) {
+	thread.start(&_thread_func, this);
+}
+
+template <class K>
+ThreadedCallableQueue<K>::~ThreadedCallableQueue() {
+	exit = true;
+	sem.post();
+	thread.wait_to_finish();
+}
+
+#endif // THREADED_CALLABLE_QUEUE_H

core/containers/tight_local_vector.h

@@ -0,0 +1,318 @@
+/**************************************************************************/
+/*  local_vector.h                                                        */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef TIGHT_LOCAL_VECTOR_H
+#define TIGHT_LOCAL_VECTOR_H
+
+#include "core/containers/pool_vector.h"
+#include "core/containers/sort_array.h"
+#include "core/containers/vector.h"
+#include "core/error/error_macros.h"
+#include "core/os/memory.h"
+
+// It grows strictly as much as needed. (The vanilla LocalVector is what you want in most cases).
+template <class T, class U = uint32_t, bool force_trivial = false>
+class TightLocalVector {
+private:
+	U count = 0;
+	U capacity = 0;
+	T *data = nullptr;
+
+public:
+	T *ptr() {
+		return data;
+	}
+
+	const T *ptr() const {
+		return data;
+	}
+
+	_FORCE_INLINE_ void push_back(T p_elem) {
+		if (unlikely(count == capacity)) {
+			if (capacity == 0) {
+				capacity = 1;
+			} else {
+				capacity <<= 1;
+			}
+			data = (T *)memrealloc(data, capacity * sizeof(T));
+			CRASH_COND_MSG(!data, "Out of memory");
+		}
+
+		if constexpr (!HAS_TRIVIAL_CONSTRUCTOR(T) && !force_trivial) {
+			memnew_placement(&data[count++], T(p_elem));
+		} else {
+			data[count++] = p_elem;
+		}
+	}
+
+	void remove(U p_index) {
+		ERR_FAIL_UNSIGNED_INDEX(p_index, count);
+		count--;
+		for (U i = p_index; i < count; i++) {
+			data[i] = data[i + 1];
+		}
+		if constexpr (!HAS_TRIVIAL_DESTRUCTOR(T) && !force_trivial) {
+			data[count].~T();
+		}
+	}
+
+	/// Removes the item copying the last value into the position of the one to
+	/// remove. It's generally faster than `remove`.
+	void remove_unordered(U p_index) {
+		ERR_FAIL_INDEX(p_index, count);
+		count--;
+		if (count > p_index) {
+			data[p_index] = data[count];
+		}
+		if constexpr (!HAS_TRIVIAL_DESTRUCTOR(T) && !force_trivial) {
+			data[count].~T();
+		}
+	}
+
+	void erase(const T &p_val) {
+		int64_t idx = find(p_val);
+		if (idx >= 0) {
+			remove(idx);
+		}
+	}
+
+	U erase_multiple_unordered(const T &p_val) {
+		U from = 0;
+		U count = 0;
+		while (true) {
+			int64_t idx = find(p_val, from);
+
+			if (idx == -1) {
+				break;
+			}
+			remove_unordered(idx);
+			from = idx;
+			count++;
+		}
+		return count;
+	}
+
+	void invert() {
+		for (U i = 0; i < count / 2; i++) {
+			SWAP(data[i], data[count - i - 1]);
+		}
+	}
+
+	_FORCE_INLINE_ void clear() { resize(0); }
+	_FORCE_INLINE_ void reset() {
+		clear();
+		if (data) {
+			memfree(data);
+			data = nullptr;
+			capacity = 0;
+		}
+	}
+	_FORCE_INLINE_ bool empty() const { return count == 0; }
+	_FORCE_INLINE_ U get_capacity() const { return capacity; }
+	_FORCE_INLINE_ void reserve(U p_size) {
+		if (p_size > capacity) {
+			capacity = p_size;
+			data = (T *)memrealloc(data, capacity * sizeof(T));
+			CRASH_COND_MSG(!data, "Out of memory");
+		}
+	}
+
+	_FORCE_INLINE_ U size() const { return count; }
+	void resize(U p_size) {
+		if (p_size < count) {
+			if (!HAS_TRIVIAL_DESTRUCTOR(T) && !force_trivial) {
+				for (U i = p_size; i < count; i++) {
+					data[i].~T();
+				}
+			}
+			count = p_size;
+		} else if (p_size > count) {
+			if (unlikely(p_size > capacity)) {
+				if (capacity == 0) {
+					capacity = 1;
+				}
+				while (capacity < p_size) {
+					capacity <<= 1;
+				}
+				data = (T *)memrealloc(data, capacity * sizeof(T));
+				CRASH_COND_MSG(!data, "Out of memory");
+			}
+			if (!HAS_TRIVIAL_CONSTRUCTOR(T) && !force_trivial) {
+				for (U i = count; i < p_size; i++) {
+					memnew_placement(&data[i], T);
+				}
+			}
+			count = p_size;
+		}
+	}
+	_FORCE_INLINE_ const T &operator[](U p_index) const {
+		CRASH_BAD_UNSIGNED_INDEX(p_index, count);
+		return data[p_index];
+	}
+	_FORCE_INLINE_ T &operator[](U p_index) {
+		CRASH_BAD_UNSIGNED_INDEX(p_index, count);
+		return data[p_index];
+	}
+
+	void fill(T p_val) {
+		for (U i = 0; i < count; i++) {
+			data[i] = p_val;
+		}
+	}
+
+	void insert(U p_pos, T p_val) {
+		ERR_FAIL_UNSIGNED_INDEX(p_pos, count + 1);
+		if (p_pos == count) {
+			push_back(p_val);
+		} else {
+			resize(count + 1);
+			for (U i = count - 1; i > p_pos; i--) {
+				data[i] = data[i - 1];
+			}
+			data[p_pos] = p_val;
+		}
+	}
+
+	int64_t find(const T &p_val, U p_from = 0) const {
+		for (U i = p_from; i < count; i++) {
+			if (data[i] == p_val) {
+				return int64_t(i);
+			}
+		}
+		return -1;
+	}
+
+	template <class C>
+	void sort_custom() {
+		U len = count;
+		if (len == 0) {
+			return;
+		}
+
+		SortArray<T, C> sorter;
+		sorter.sort(data, len);
+	}
+
+	void sort() {
+		sort_custom<_DefaultComparator<T>>();
+	}
+
+	void ordered_insert(T p_val) {
+		U i;
+		for (i = 0; i < count; i++) {
+			if (p_val < data[i]) {
+				break;
+			}
+		}
+		insert(i, p_val);
+	}
+
+	operator Vector<T>() const {
+		Vector<T> ret;
+		ret.resize(size());
+		T *w = ret.ptrw();
+		memcpy(w, data, sizeof(T) * count);
+		return ret;
+	}
+
+	operator PoolVector<T>() const {
+		PoolVector<T> pl;
+		if (size()) {
+			pl.resize(size());
+			typename PoolVector<T>::Write w = pl.write();
+			T *dest = w.ptr();
+			memcpy(dest, data, sizeof(T) * count);
+		}
+		return pl;
+	}
+
+	Vector<uint8_t> to_byte_array() const { //useful to pass stuff to gpu or variant
+		Vector<uint8_t> ret;
+		ret.resize(count * sizeof(T));
+		uint8_t *w = ret.ptrw();
+		memcpy(w, data, sizeof(T) * count);
+		return ret;
+	}
+
+	_FORCE_INLINE_ TightLocalVector() {}
+	_FORCE_INLINE_ TightLocalVector(const TightLocalVector &p_from) {
+		resize(p_from.size());
+		for (U i = 0; i < p_from.count; i++) {
+			data[i] = p_from.data[i];
+		}
+	}
+	TightLocalVector(const Vector<T> &p_from) {
+		resize(p_from.size());
+		for (U i = 0; i < count; i++) {
+			data[i] = p_from[i];
+		}
+	}
+	TightLocalVector(const PoolVector<T> &p_from) {
+		resize(p_from.size());
+		typename PoolVector<T>::Read r = p_from.read();
+		for (U i = 0; i < count; i++) {
+			data[i] = r[i];
+		}
+	}
+
+	inline void operator=(const TightLocalVector &p_from) {
+		resize(p_from.size());
+		for (U i = 0; i < p_from.count; i++) {
+			data[i] = p_from.data[i];
+		}
+	}
+	inline void operator=(const Vector<T> &p_from) {
+		resize(p_from.size());
+		for (U i = 0; i < count; i++) {
+			data[i] = p_from[i];
+		}
+	}
+	inline TightLocalVector &operator=(const PoolVector<T> &p_from) {
+		resize(p_from.size());
+		typename PoolVector<T>::Read r = p_from.read();
+		for (U i = 0; i < count; i++) {
+			data[i] = r[i];
+		}
+		return *this;
+	}
+
+	_FORCE_INLINE_ ~TightLocalVector() {
+		if (data) {
+			reset();
+		}
+	}
+};
+
+// Integer default version
+template <class T, class I = int32_t, bool force_trivial = false>
+class TightLocalVectori : public TightLocalVector<T, I, force_trivial> {
+};
+
+#endif // TIGHT_LOCAL_VECTOR_H

core/containers/vector.h

@@ -0,0 +1,196 @@
+#ifndef VECTOR_H
+#define VECTOR_H
+/*************************************************************************/
+/*  vector.h                                                             */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+/**
+ * @class Vector
+ * @author Juan Linietsky
+ * Vector container. Regular Vector Container. Use with care and for smaller arrays when possible. Use PoolVector for large arrays.
+ */
+
+#include "core/containers/cowdata.h"
+#include "core/containers/sort_array.h"
+#include "core/error/error_macros.h"
+#include "core/os/memory.h"
+
+template <class T>
+class VectorWriteProxy {
+public:
+	_FORCE_INLINE_ T &operator[](int p_index) {
+		CRASH_BAD_INDEX(p_index, ((Vector<T> *)(this))->_cowdata.size());
+
+		return ((Vector<T> *)(this))->_cowdata.ptrw()[p_index];
+	}
+};
+
+template <class T>
+class Vector {
+	friend class VectorWriteProxy<T>;
+
+public:
+	VectorWriteProxy<T> write;
+
+private:
+	CowData<T> _cowdata;
+
+public:
+	bool push_back(T p_elem);
+
+	void remove(int p_index) { _cowdata.remove(p_index); }
+	_FORCE_INLINE_ bool erase(const T &p_val) {
+		int idx = find(p_val);
+		if (idx >= 0) {
+			remove(idx);
+			return true;
+		}
+		return false;
+	};
+	void invert();
+
+	_FORCE_INLINE_ T *ptrw() { return _cowdata.ptrw(); }
+	_FORCE_INLINE_ const T *ptr() const { return _cowdata.ptr(); }
+	_FORCE_INLINE_ void clear() { resize(0); }
+	_FORCE_INLINE_ bool empty() const { return _cowdata.empty(); }
+
+	_FORCE_INLINE_ T get(int p_index) { return _cowdata.get(p_index); }
+	_FORCE_INLINE_ const T &get(int p_index) const { return _cowdata.get(p_index); }
+	_FORCE_INLINE_ void set(int p_index, const T &p_elem) { _cowdata.set(p_index, p_elem); }
+	_FORCE_INLINE_ int size() const { return _cowdata.size(); }
+	Error resize(int p_size) { return _cowdata.resize(p_size); }
+	_FORCE_INLINE_ const T &operator[](int p_index) const { return _cowdata.get(p_index); }
+	Error insert(int p_pos, T p_val) { return _cowdata.insert(p_pos, p_val); }
+	int find(const T &p_val, int p_from = 0) const { return _cowdata.find(p_val, p_from); }
+	_FORCE_INLINE_ void fill(const T &p_val) { _cowdata.fill(p_val); }
+
+	void append_array(Vector<T> p_other);
+
+	template <class C>
+	void sort_custom() {
+		int len = _cowdata.size();
+		if (len == 0) {
+			return;
+		}
+
+		T *data = ptrw();
+		SortArray<T, C> sorter;
+		sorter.sort(data, len);
+	}
+
+	void sort() {
+		sort_custom<_DefaultComparator<T>>();
+	}
+
+	void ordered_insert(const T &p_val) {
+		int i;
+		for (i = 0; i < _cowdata.size(); i++) {
+			if (p_val < operator[](i)) {
+				break;
+			};
+		};
+		insert(i, p_val);
+	}
+
+	_FORCE_INLINE_ Vector() {}
+	_FORCE_INLINE_ Vector(const Vector &p_from) { _cowdata._ref(p_from._cowdata); }
+	inline Vector &operator=(const Vector &p_from) {
+		_cowdata._ref(p_from._cowdata);
+		return *this;
+	}
+
+	Vector<uint8_t> to_byte_array() const {
+		Vector<uint8_t> ret;
+		ret.resize(size() * sizeof(T));
+		memcpy(ret.ptrw(), ptr(), sizeof(T) * size());
+		return ret;
+	}
+
+	Vector<T> slice(int p_begin, int p_end = INT32_MAX) const {
+		Vector<T> result;
+
+		const int s = size();
+
+		int begin = CLAMP(p_begin, -s, s);
+		if (begin < 0) {
+			begin += s;
+		}
+		int end = CLAMP(p_end, -s, s);
+		if (end < 0) {
+			end += s;
+		}
+
+		ERR_FAIL_COND_V(begin > end, result);
+
+		int result_size = end - begin;
+		result.resize(result_size);
+
+		const T *const r = ptr();
+		T *const w = result.ptrw();
+		for (int i = 0; i < result_size; ++i) {
+			w[i] = r[begin + i];
+		}
+
+		return result;
+	}
+
+	_FORCE_INLINE_ ~Vector() {}
+};
+
+template <class T>
+void Vector<T>::invert() {
+	for (int i = 0; i < size() / 2; i++) {
+		T *p = ptrw();
+		SWAP(p[i], p[size() - i - 1]);
+	}
+}
+
+template <class T>
+void Vector<T>::append_array(Vector<T> p_other) {
+	const int ds = p_other.size();
+	if (ds == 0) {
+		return;
+	}
+	const int bs = size();
+	resize(bs + ds);
+	for (int i = 0; i < ds; ++i) {
+		ptrw()[bs + i] = p_other[i];
+	}
+}
+
+template <class T>
+bool Vector<T>::push_back(T p_elem) {
+	Error err = resize(size() + 1);
+	ERR_FAIL_COND_V(err, true);
+	set(size() - 1, p_elem);
+
+	return false;
+}
+
+#endif

core/containers/vmap.h

@@ -0,0 +1,201 @@
+#ifndef VMAP_H
+#define VMAP_H
+/*************************************************************************/
+/*  vmap.h                                                               */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "core/containers/cowdata.h"
+#include "core/typedefs.h"
+
+template <class T, class V>
+class VMap {
+public:
+	struct Pair {
+		T key;
+		V value;
+
+		_FORCE_INLINE_ Pair() {}
+
+		_FORCE_INLINE_ Pair(const T &p_key, const V &p_value) {
+			key = p_key;
+			value = p_value;
+		}
+	};
+
+private:
+	CowData<Pair> _cowdata;
+
+	_FORCE_INLINE_ int _find(const T &p_val, bool &r_exact) const {
+		r_exact = false;
+		if (_cowdata.empty()) {
+			return 0;
+		}
+
+		int low = 0;
+		int high = _cowdata.size() - 1;
+		const Pair *a = _cowdata.ptr();
+		int middle = 0;
+
+#ifdef DEBUG_ENABLED
+		if (low > high)
+			ERR_PRINT("low > high, this may be a bug");
+#endif
+		while (low <= high) {
+			middle = (low + high) / 2;
+
+			if (p_val < a[middle].key) {
+				high = middle - 1; //search low end of array
+			} else if (a[middle].key < p_val) {
+				low = middle + 1; //search high end of array
+			} else {
+				r_exact = true;
+				return middle;
+			}
+		}
+
+		//return the position where this would be inserted
+		if (a[middle].key < p_val) {
+			middle++;
+		}
+		return middle;
+	}
+
+	_FORCE_INLINE_ int _find_exact(const T &p_val) const {
+		if (_cowdata.empty()) {
+			return -1;
+		}
+
+		int low = 0;
+		int high = _cowdata.size() - 1;
+		int middle;
+		const Pair *a = _cowdata.ptr();
+
+		while (low <= high) {
+			middle = (low + high) / 2;
+
+			if (p_val < a[middle].key) {
+				high = middle - 1; //search low end of array
+			} else if (a[middle].key < p_val) {
+				low = middle + 1; //search high end of array
+			} else {
+				return middle;
+			}
+		}
+
+		return -1;
+	}
+
+public:
+	int insert(const T &p_key, const V &p_val) {
+		bool exact;
+		int pos = _find(p_key, exact);
+		if (exact) {
+			_cowdata.get_m(pos).value = p_val;
+			return pos;
+		}
+		_cowdata.insert(pos, Pair(p_key, p_val));
+		return pos;
+	}
+
+	bool has(const T &p_val) const {
+		return _find_exact(p_val) != -1;
+	}
+
+	void erase(const T &p_val) {
+		int pos = _find_exact(p_val);
+		if (pos < 0) {
+			return;
+		}
+		_cowdata.remove(pos);
+	}
+
+	int find(const T &p_val) const {
+		return _find_exact(p_val);
+	}
+
+	int find_nearest(const T &p_val) const {
+		if (_cowdata.empty()) {
+			return -1;
+		}
+		bool exact;
+		return _find(p_val, exact);
+	}
+
+	_FORCE_INLINE_ int size() const { return _cowdata.size(); }
+	_FORCE_INLINE_ bool empty() const { return _cowdata.empty(); }
+
+	const Pair *get_array() const {
+		return _cowdata.ptr();
+	}
+
+	Pair *get_array() {
+		return _cowdata.ptrw();
+	}
+
+	const V &getv(int p_index) const {
+		return _cowdata.get(p_index).value;
+	}
+
+	V &getv(int p_index) {
+		return _cowdata.get_m(p_index).value;
+	}
+
+	const T &getk(int p_index) const {
+		return _cowdata.get(p_index).key;
+	}
+
+	T &getk(int p_index) {
+		return _cowdata.get_m(p_index).key;
+	}
+
+	inline const V &operator[](const T &p_key) const {
+		int pos = _find_exact(p_key);
+
+		CRASH_COND(pos < 0);
+
+		return _cowdata.get(pos).value;
+	}
+
+	inline V &operator[](const T &p_key) {
+		int pos = _find_exact(p_key);
+		if (pos < 0) {
+			pos = insert(p_key, V());
+		}
+
+		return _cowdata.get_m(pos).value;
+	}
+
+	_FORCE_INLINE_ VMap(){};
+	_FORCE_INLINE_ VMap(const VMap &p_from) { _cowdata._ref(p_from._cowdata); }
+	inline VMap &operator=(const VMap &p_from) {
+		_cowdata._ref(p_from._cowdata);
+		return *this;
+	}
+};
+#endif // VMAP_H

core/containers/vset.h

@@ -0,0 +1,140 @@
+#ifndef VSET_H
+#define VSET_H
+/*************************************************************************/
+/*  vset.h                                                               */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "core/typedefs.h"
+#include "core/containers/vector.h"
+
+template <class T>
+class VSet {
+	Vector<T> _data;
+
+	_FORCE_INLINE_ int _find(const T &p_val, bool &r_exact) const {
+		r_exact = false;
+		if (_data.empty()) {
+			return 0;
+		}
+
+		int low = 0;
+		int high = _data.size() - 1;
+		const T *a = &_data[0];
+		int middle = 0;
+
+#ifdef DEBUG_ENABLED
+		if (low > high)
+			ERR_PRINT("low > high, this may be a bug");
+#endif
+
+		while (low <= high) {
+			middle = (low + high) / 2;
+
+			if (p_val < a[middle]) {
+				high = middle - 1; //search low end of array
+			} else if (a[middle] < p_val) {
+				low = middle + 1; //search high end of array
+			} else {
+				r_exact = true;
+				return middle;
+			}
+		}
+
+		//return the position where this would be inserted
+		if (a[middle] < p_val) {
+			middle++;
+		}
+		return middle;
+	}
+
+	_FORCE_INLINE_ int _find_exact(const T &p_val) const {
+		if (_data.empty()) {
+			return -1;
+		}
+
+		int low = 0;
+		int high = _data.size() - 1;
+		int middle;
+		const T *a = &_data[0];
+
+		while (low <= high) {
+			middle = (low + high) / 2;
+
+			if (p_val < a[middle]) {
+				high = middle - 1; //search low end of array
+			} else if (a[middle] < p_val) {
+				low = middle + 1; //search high end of array
+			} else {
+				return middle;
+			}
+		}
+
+		return -1;
+	}
+
+public:
+	void insert(const T &p_val) {
+		bool exact;
+		int pos = _find(p_val, exact);
+		if (exact) {
+			return;
+		}
+		_data.insert(pos, p_val);
+	}
+
+	bool has(const T &p_val) const {
+		return _find_exact(p_val) != -1;
+	}
+
+	void erase(const T &p_val) {
+		int pos = _find_exact(p_val);
+		if (pos < 0) {
+			return;
+		}
+		_data.remove(pos);
+	}
+
+	int find(const T &p_val) const {
+		return _find_exact(p_val);
+	}
+
+	_FORCE_INLINE_ bool empty() const { return _data.empty(); }
+
+	_FORCE_INLINE_ int size() const { return _data.size(); }
+
+	inline T &operator[](int p_index) {
+		return _data.write[p_index];
+	}
+
+	inline const T &operator[](int p_index) const {
+		return _data[p_index];
+	}
+};
+
+#endif // VSET_H