17 files changed, 733 insertions, 217 deletions

diff --git a/core/templates/bin_sorted_array.h b/core/templates/bin_sorted_array.h
new file mode 100644
index 0000000000..be9d0b5475
--- /dev/null
+++ b/core/templates/bin_sorted_array.h
@@ -0,0 +1,181 @@
+/*************************************************************************/
+/*  bin_sorted_array.h                                                   */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2021 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 BIN_SORTED_ARRAY_H
+#define BIN_SORTED_ARRAY_H
+
+#include "core/templates/local_vector.h"
+#include "core/templates/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_COND_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(uint64_t p_idx) {
+		ERR_FAIL_COND(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
diff --git a/core/templates/command_queue_mt.cpp b/core/templates/command_queue_mt.cpp
index 238bf3975c..04a8095f0b 100644
--- a/core/templates/command_queue_mt.cpp
+++ b/core/templates/command_queue_mt.cpp
@@ -70,35 +70,7 @@ CommandQueueMT::SyncSemaphore *CommandQueueMT::_alloc_sync_sem() {
 	return &sync_sems[idx];
 }
 
-bool CommandQueueMT::dealloc_one() {
-tryagain:
-	if (dealloc_ptr == (write_ptr_and_epoch >> 1)) {
-		// The queue is empty
-		return false;
-	}
-
-	uint32_t size = *(uint32_t *)&command_mem[dealloc_ptr];
-
-	if (size == 0) {
-		// End of command buffer wrap down
-		dealloc_ptr = 0;
-		goto tryagain;
-	}
-
-	if (size & 1) {
-		// Still used, nothing can be deallocated
-		return false;
-	}
-
-	dealloc_ptr += (size >> 1) + 8;
-	return true;
-}
-
 CommandQueueMT::CommandQueueMT(bool p_sync) {
-	command_mem_size = GLOBAL_DEF_RST("memory/limits/command_queue/multithreading_queue_size_kb", DEFAULT_COMMAND_MEM_SIZE_KB);
-	ProjectSettings::get_singleton()->set_custom_property_info("memory/limits/command_queue/multithreading_queue_size_kb", PropertyInfo(Variant::INT, "memory/limits/command_queue/multithreading_queue_size_kb", PROPERTY_HINT_RANGE, "1,4096,1,or_greater"));
-	command_mem_size *= 1024;
-	command_mem = (uint8_t *)memalloc(command_mem_size);
 	if (p_sync) {
 		sync = memnew(Semaphore);
 	}
@@ -108,5 +80,4 @@ CommandQueueMT::~CommandQueueMT() {
 	if (sync) {
 		memdelete(sync);
 	}
-	memfree(command_mem);
 }
diff --git a/core/templates/command_queue_mt.h b/core/templates/command_queue_mt.h
index 0012cea72d..519a896ffc 100644
--- a/core/templates/command_queue_mt.h
+++ b/core/templates/command_queue_mt.h
@@ -34,6 +34,8 @@
 #include "core/os/memory.h"
 #include "core/os/mutex.h"
 #include "core/os/semaphore.h"
+#include "core/string/print_string.h"
+#include "core/templates/local_vector.h"
 #include "core/templates/simple_type.h"
 #include "core/typedefs.h"
 
@@ -319,7 +321,7 @@ class CommandQueueMT {
 	DECL_CMD(0)
 	SPACE_SEP_LIST(DECL_CMD, 15)
 
-	/* comands that return */
+	// Commands that return.
 	DECL_CMD_RET(0)
 	SPACE_SEP_LIST(DECL_CMD_RET, 15)
 
@@ -334,11 +336,7 @@ class CommandQueueMT {
 		SYNC_SEMAPHORES = 8
 	};
 
-	uint8_t *command_mem = nullptr;
-	uint32_t read_ptr_and_epoch = 0;
-	uint32_t write_ptr_and_epoch = 0;
-	uint32_t dealloc_ptr = 0;
-	uint32_t command_mem_size = 0;
+	LocalVector<uint8_t> command_mem;
 	SyncSemaphore sync_sems[SYNC_SEMAPHORES];
 	Mutex mutex;
 	Semaphore *sync = nullptr;
@@ -346,138 +344,47 @@ class CommandQueueMT {
 	template <class T>
 	T *allocate() {
 		// alloc size is size+T+safeguard
-		uint32_t alloc_size = ((sizeof(T) + 8 - 1) & ~(8 - 1)) + 8;
-
-		// Assert that the buffer is big enough to hold at least two messages.
-		ERR_FAIL_COND_V(alloc_size * 2 + sizeof(uint32_t) > command_mem_size, nullptr);
-
-	tryagain:
-		uint32_t write_ptr = write_ptr_and_epoch >> 1;
-
-		if (write_ptr < dealloc_ptr) {
-			// behind dealloc_ptr, check that there is room
-			if ((dealloc_ptr - write_ptr) <= alloc_size) {
-				// There is no more room, try to deallocate something
-				if (dealloc_one()) {
-					goto tryagain;
-				}
-				return nullptr;
-			}
-		} else {
-			// ahead of dealloc_ptr, check that there is room
-
-			if ((command_mem_size - write_ptr) < alloc_size + sizeof(uint32_t)) {
-				// no room at the end, wrap down;
-
-				if (dealloc_ptr == 0) { // don't want write_ptr to become dealloc_ptr
-
-					// There is no more room, try to deallocate something
-					if (dealloc_one()) {
-						goto tryagain;
-					}
-					return nullptr;
-				}
-
-				// if this happens, it's a bug
-				ERR_FAIL_COND_V((command_mem_size - write_ptr) < 8, nullptr);
-				// zero means, wrap to beginning
-
-				uint32_t *p = (uint32_t *)&command_mem[write_ptr];
-				*p = 1;
-				write_ptr_and_epoch = 0 | (1 & ~write_ptr_and_epoch); // Invert epoch.
-				// See if we can get the thread to run and clear up some more space while we wait.
-				// This is required if alloc_size * 2 + 4 > COMMAND_MEM_SIZE
-				if (sync) {
-					sync->post();
-				}
-				goto tryagain;
-			}
-		}
-		// Allocate the size and the 'in use' bit.
-		// First bit used to mark if command is still in use (1)
-		// or if it has been destroyed and can be deallocated (0).
-		uint32_t size = (sizeof(T) + 8 - 1) & ~(8 - 1);
-		uint32_t *p = (uint32_t *)&command_mem[write_ptr];
-		*p = (size << 1) | 1;
-		write_ptr += 8;
-		// allocate the command
-		T *cmd = memnew_placement(&command_mem[write_ptr], T);
-		write_ptr += size;
-		write_ptr_and_epoch = (write_ptr << 1) | (write_ptr_and_epoch & 1);
+		uint32_t alloc_size = ((sizeof(T) + 8 - 1) & ~(8 - 1));
+		uint64_t size = command_mem.size();
+		command_mem.resize(size + alloc_size + 8);
+		*(uint64_t *)&command_mem[size] = alloc_size;
+		T *cmd = memnew_placement(&command_mem[size + 8], T);
 		return cmd;
 	}
 
 	template <class T>
 	T *allocate_and_lock() {
 		lock();
-		T *ret;
-
-		while ((ret = allocate<T>()) == nullptr) {
-			unlock();
-			// sleep a little until fetch happened and some room is made
-			wait_for_flush();
-			lock();
-		}
-
+		T *ret = allocate<T>();
 		return ret;
 	}
 
-	bool flush_one(bool p_lock = true) {
-		if (p_lock) {
-			lock();
-		}
-	tryagain:
-
-		// tried to read an empty queue
-		if (read_ptr_and_epoch == write_ptr_and_epoch) {
-			if (p_lock) {
-				unlock();
-			}
-			return false;
-		}
-
-		uint32_t read_ptr = read_ptr_and_epoch >> 1;
-		uint32_t size_ptr = read_ptr;
-		uint32_t size = *(uint32_t *)&command_mem[read_ptr] >> 1;
-
-		if (size == 0) {
-			*(uint32_t *)&command_mem[read_ptr] = 0; // clear in-use bit.
-			//end of ringbuffer, wrap
-			read_ptr_and_epoch = 0 | (1 & ~read_ptr_and_epoch); // Invert epoch.
-			goto tryagain;
-		}
-
-		read_ptr += 8;
+	void _flush() {
+		lock();
 
-		CommandBase *cmd = reinterpret_cast<CommandBase *>(&command_mem[read_ptr]);
+		uint64_t read_ptr = 0;
+		uint64_t limit = command_mem.size();
 
-		read_ptr += size;
+		while (read_ptr < limit) {
+			uint64_t size = *(uint64_t *)&command_mem[read_ptr];
+			read_ptr += 8;
+			CommandBase *cmd = reinterpret_cast<CommandBase *>(&command_mem[read_ptr]);
 
-		read_ptr_and_epoch = (read_ptr << 1) | (read_ptr_and_epoch & 1);
+			cmd->call(); //execute the function
+			cmd->post(); //release in case it needs sync/ret
+			cmd->~CommandBase(); //should be done, so erase the command
 
-		if (p_lock) {
-			unlock();
-		}
-		cmd->call();
-		if (p_lock) {
-			lock();
+			read_ptr += size;
 		}
 
-		cmd->post();
-		cmd->~CommandBase();
-		*(uint32_t *)&command_mem[size_ptr] &= ~1;
-
-		if (p_lock) {
-			unlock();
-		}
-		return true;
+		command_mem.clear();
+		unlock();
 	}
 
 	void lock();
 	void unlock();
 	void wait_for_flush();
 	SyncSemaphore *_alloc_sync_sem();
-	bool dealloc_one();
 
 public:
 	/* NORMAL PUSH COMMANDS */
@@ -492,23 +399,19 @@ public:
 	DECL_PUSH_AND_SYNC(0)
 	SPACE_SEP_LIST(DECL_PUSH_AND_SYNC, 15)
 
-	void wait_and_flush_one() {
-		ERR_FAIL_COND(!sync);
-		sync->wait();
-		flush_one();
-	}
-
 	_FORCE_INLINE_ void flush_if_pending() {
-		if (unlikely(read_ptr_and_epoch != write_ptr_and_epoch)) {
-			flush_all();
+		if (unlikely(command_mem.size() > 0)) {
+			_flush();
 		}
 	}
 	void flush_all() {
-		//ERR_FAIL_COND(sync);
-		lock();
-		while (flush_one(false)) {
-		}
-		unlock();
+		_flush();
+	}
+
+	void wait_and_flush() {
+		ERR_FAIL_COND(!sync);
+		sync->wait();
+		_flush();
 	}
 
 	CommandQueueMT(bool p_sync);
diff --git a/core/templates/cowdata.h b/core/templates/cowdata.h
index c985593473..ba9babe0af 100644
--- a/core/templates/cowdata.h
+++ b/core/templates/cowdata.h
@@ -232,7 +232,7 @@ uint32_t CowData<T>::_copy_on_write() {
 
 		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
+		new (mem_new - 2) SafeNumeric<uint32_t>(1); //refcount
 		*(mem_new - 1) = current_size; //size
 
 		T *_data = (T *)(mem_new);
@@ -286,14 +286,14 @@ Error CowData<T>::resize(int p_size) {
 				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
+				new (ptr - 2) 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
+				new (_ptrnew - 2) SafeNumeric<uint32_t>(rc); //refcount
 
 				_ptr = (T *)(_ptrnew);
 			}
@@ -323,7 +323,7 @@ Error CowData<T>::resize(int p_size) {
 		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
+			new (_ptrnew - 2) SafeNumeric<uint32_t>(rc); //refcount
 
 			_ptr = (T *)(_ptrnew);
 		}
diff --git a/core/templates/hash_map.h b/core/templates/hash_map.h
index dc378aed69..1257b54449 100644
--- a/core/templates/hash_map.h
+++ b/core/templates/hash_map.h
@@ -291,7 +291,7 @@ public:
 	}
 
 	/**
-	 * Same as get, except it can return nullptr  when item was not found.
+	 * Same as get, except it can return nullptr when item was not found.
 	 * This is mainly used for speed purposes.
 	 */
 
@@ -324,7 +324,7 @@ public:
 	}
 
 	/**
-	 * Same as get, except it can return nullptr  when item was not found.
+	 * Same as get, except it can return nullptr when item was not found.
 	 * This version is custom, will take a hash and a custom key (that should support operator==()
 	 */
 
@@ -443,7 +443,7 @@ public:
 
 	/**
 	 * 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, nullptr  otherwise.
+	 * Returns a pointer to the next key if found, nullptr otherwise.
 	 * Adding/Removing elements while iterating will, of course, have unexpected results, don't do it.
 	 *
 	 * Example:
diff --git a/core/templates/hashfuncs.h b/core/templates/hashfuncs.h
index 4572b269cf..2e932f9f26 100644
--- a/core/templates/hashfuncs.h
+++ b/core/templates/hashfuncs.h
@@ -95,7 +95,7 @@ static inline uint32_t hash_djb2_one_float(double p_in, uint32_t p_prev = 5381)
 	if (p_in == 0.0f) {
 		u.d = 0.0;
 	} else if (Math::is_nan(p_in)) {
-		u.d = Math_NAN;
+		u.d = NAN;
 	} else {
 		u.d = p_in;
 	}
@@ -124,7 +124,7 @@ static inline uint64_t hash_djb2_one_float_64(double p_in, uint64_t p_prev = 538
 	if (p_in == 0.0f) {
 		u.d = 0.0;
 	} else if (Math::is_nan(p_in)) {
-		u.d = Math_NAN;
+		u.d = NAN;
 	} else {
 		u.d = p_in;
 	}
diff --git a/core/templates/list.h b/core/templates/list.h
index 010e35eed8..c2e17a2f6f 100644
--- a/core/templates/list.h
+++ b/core/templates/list.h
@@ -135,6 +135,83 @@ public:
 		_FORCE_INLINE_ Element() {}
 	};
 
+	typedef T ValueType;
+
+	struct Iterator {
+		_FORCE_INLINE_ T &operator*() const {
+			return E->get();
+		}
+		_FORCE_INLINE_ T *operator->() const { return &E->get(); }
+		_FORCE_INLINE_ Iterator &operator++() {
+			E = E->next();
+			return *this;
+		}
+		_FORCE_INLINE_ Iterator &operator--() {
+			E = E->prev();
+			return *this;
+		}
+
+		_FORCE_INLINE_ bool operator==(const Iterator &b) const { return E == b.E; }
+		_FORCE_INLINE_ bool operator!=(const Iterator &b) const { return E != b.E; }
+
+		Iterator(Element *p_E) { E = p_E; }
+		Iterator() {}
+		Iterator(const Iterator &p_it) { E = p_it.E; }
+
+	private:
+		Element *E = nullptr;
+	};
+
+	struct ConstIterator {
+		_FORCE_INLINE_ const T &operator*() const {
+			return E->get();
+		}
+		_FORCE_INLINE_ const T *operator->() const { return &E->get(); }
+		_FORCE_INLINE_ ConstIterator &operator++() {
+			E = E->next();
+			return *this;
+		}
+		_FORCE_INLINE_ ConstIterator &operator--() {
+			E = E->prev();
+			return *this;
+		}
+
+		_FORCE_INLINE_ bool operator==(const ConstIterator &b) const { return E == b.E; }
+		_FORCE_INLINE_ bool operator!=(const ConstIterator &b) const { return E != b.E; }
+
+		_FORCE_INLINE_ ConstIterator(const Element *p_E) { E = p_E; }
+		_FORCE_INLINE_ ConstIterator() {}
+		_FORCE_INLINE_ ConstIterator(const ConstIterator &p_it) { E = p_it.E; }
+
+	private:
+		const Element *E = nullptr;
+	};
+
+	_FORCE_INLINE_ Iterator begin() {
+		return Iterator(front());
+	}
+	_FORCE_INLINE_ Iterator end() {
+		return Iterator(nullptr);
+	}
+
+#if 0
+	//to use when replacing find()
+	_FORCE_INLINE_ Iterator find(const K &p_key) {
+		return Iterator(find(p_key));
+	}
+#endif
+	_FORCE_INLINE_ ConstIterator begin() const {
+		return ConstIterator(front());
+	}
+	_FORCE_INLINE_ ConstIterator end() const {
+		return ConstIterator(nullptr);
+	}
+#if 0
+	//to use when replacing find()
+	_FORCE_INLINE_ ConstIterator find(const K &p_key) const {
+		return ConstIterator(find(p_key));
+	}
+#endif
 private:
 	struct _Data {
 		Element *first = nullptr;
diff --git a/core/templates/local_vector.h b/core/templates/local_vector.h
index 5f22e08eb8..668ec513d6 100644
--- a/core/templates/local_vector.h
+++ b/core/templates/local_vector.h
@@ -178,7 +178,7 @@ public:
 	}
 
 	int64_t find(const T &p_val, U p_from = 0) const {
-		for (U i = 0; i < count; i++) {
+		for (U i = p_from; i < count; i++) {
 			if (data[i] == p_val) {
 				return int64_t(i);
 			}
diff --git a/core/templates/map.h b/core/templates/map.h
index 7dfee13d2c..a47547d355 100644
--- a/core/templates/map.h
+++ b/core/templates/map.h
@@ -33,6 +33,7 @@
 
 #include "core/error/error_macros.h"
 #include "core/os/memory.h"
+#include "core/templates/pair.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
@@ -55,11 +56,12 @@ public:
 		Element *parent = nullptr;
 		Element *_next = nullptr;
 		Element *_prev = nullptr;
-		K _key;
-		V _value;
-		//_Data *data;
+		KeyValue<K, V> _data;
 
 	public:
+		KeyValue<K, V> &key_value() { return _data; }
+		const KeyValue<K, V> &key_value() const { return _data; }
+
 		const Element *next() const {
 			return _next;
 		}
@@ -73,23 +75,106 @@ public:
 			return _prev;
 		}
 		const K &key() const {
-			return _key;
+			return _data.key;
 		}
 		V &value() {
-			return _value;
+			return _data.value;
 		}
 		const V &value() const {
-			return _value;
+			return _data.value;
 		}
 		V &get() {
-			return _value;
+			return _data.value;
 		}
 		const V &get() const {
-			return _value;
+			return _data.value;
 		}
-		Element() {}
+		Element(const KeyValue<K, V> &p_data) :
+				_data(p_data) {}
 	};
 
+	typedef KeyValue<K, V> ValueType;
+
+	struct Iterator {
+		_FORCE_INLINE_ KeyValue<K, V> &operator*() const {
+			return E->key_value();
+		}
+		_FORCE_INLINE_ KeyValue<K, V> *operator->() const { return &E->key_value(); }
+		_FORCE_INLINE_ Iterator &operator++() {
+			E = E->next();
+			return *this;
+		}
+		_FORCE_INLINE_ Iterator &operator--() {
+			E = E->prev();
+			return *this;
+		}
+
+		_FORCE_INLINE_ bool operator==(const Iterator &b) const { return E == b.E; }
+		_FORCE_INLINE_ bool operator!=(const Iterator &b) const { return E != b.E; }
+
+		Iterator(Element *p_E) { E = p_E; }
+		Iterator() {}
+		Iterator(const Iterator &p_it) { E = p_it.E; }
+
+	private:
+		Element *E = nullptr;
+	};
+
+	struct ConstIterator {
+		_FORCE_INLINE_ const KeyValue<K, V> &operator*() const {
+			return E->key_value();
+		}
+		_FORCE_INLINE_ const KeyValue<K, V> *operator->() const { return &E->key_value(); }
+		_FORCE_INLINE_ ConstIterator &operator++() {
+			E = E->next();
+			return *this;
+		}
+		_FORCE_INLINE_ ConstIterator &operator--() {
+			E = E->prev();
+			return *this;
+		}
+
+		_FORCE_INLINE_ bool operator==(const ConstIterator &b) const { return E == b.E; }
+		_FORCE_INLINE_ bool operator!=(const ConstIterator &b) const { return E != b.E; }
+
+		ConstIterator(const Element *p_E) { E = p_E; }
+		ConstIterator() {}
+		ConstIterator(const ConstIterator &p_it) { E = p_it.E; }
+
+	private:
+		const Element *E = nullptr;
+	};
+
+	_FORCE_INLINE_ Iterator begin() {
+		return Iterator(front());
+	}
+	_FORCE_INLINE_ Iterator end() {
+		return Iterator(nullptr);
+	}
+
+#if 0
+	//to use when replacing find()
+	_FORCE_INLINE_ Iterator find(const K &p_key) {
+		return Iterator(find(p_key));
+	}
+#endif
+	_FORCE_INLINE_ void remove(const Iterator &p_iter) {
+		return erase(p_iter.E);
+	}
+
+	_FORCE_INLINE_ ConstIterator begin() const {
+		return ConstIterator(front());
+	}
+	_FORCE_INLINE_ ConstIterator end() const {
+		return ConstIterator(nullptr);
+	}
+
+#if 0
+	//to use when replacing find()
+	_FORCE_INLINE_ ConstIterator find(const K &p_key) const {
+		return ConstIterator(find(p_key));
+	}
+#endif
 private:
 	struct _Data {
 		Element *_root = nullptr;
@@ -107,7 +192,7 @@ private:
 		}
 
 		void _create_root() {
-			_root = memnew_allocator(Element, A);
+			_root = memnew_allocator(Element(KeyValue<K, V>(K(), V())), A);
 			_root->parent = _root->left = _root->right = _nil;
 			_root->color = BLACK;
 		}
@@ -216,9 +301,9 @@ private:
 		C less;
 
 		while (node != _data._nil) {
-			if (less(p_key, node->_key)) {
+			if (less(p_key, node->_data.key)) {
 				node = node->left;
-			} else if (less(node->_key, p_key)) {
+			} else if (less(node->_data.key, p_key)) {
 				node = node->right;
 			} else {
 				return node; // found
@@ -236,9 +321,9 @@ private:
 		while (node != _data._nil) {
 			prev = node;
 
-			if (less(p_key, node->_key)) {
+			if (less(p_key, node->_data.key)) {
 				node = node->left;
-			} else if (less(node->_key, p_key)) {
+			} else if (less(node->_data.key, p_key)) {
 				node = node->right;
 			} else {
 				return node; // found
@@ -249,7 +334,7 @@ private:
 			return nullptr; // tree empty
 		}
 
-		if (less(p_key, prev->_key)) {
+		if (less(p_key, prev->_data.key)) {
 			prev = prev->_prev;
 		}
 
@@ -312,25 +397,25 @@ private:
 		while (node != _data._nil) {
 			new_parent = node;
 
-			if (less(p_key, node->_key)) {
+			if (less(p_key, node->_data.key)) {
 				node = node->left;
-			} else if (less(node->_key, p_key)) {
+			} else if (less(node->_data.key, p_key)) {
 				node = node->right;
 			} else {
-				node->_value = p_value;
+				node->_data.value = p_value;
 				return node; // Return existing node with new value
 			}
 		}
 
-		Element *new_node = memnew_allocator(Element, A);
+		typedef KeyValue<K, V> KV;
+		Element *new_node = memnew_allocator(Element(KV(p_key, p_value)), 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)) {
+		if (new_parent == _data._root || less(p_key, new_parent->_data.key)) {
 			new_parent->left = new_node;
 		} else {
 			new_parent->right = new_node;
@@ -575,7 +660,7 @@ public:
 		CRASH_COND(!_data._root);
 		const Element *e = find(p_key);
 		CRASH_COND(!e);
-		return e->_value;
+		return e->_data.value;
 	}
 
 	V &operator[](const K &p_key) {
@@ -588,7 +673,7 @@ public:
 			e = insert(p_key, V());
 		}
 
-		return e->_value;
+		return e->_data.value;
 	}
 
 	Element *front() const {
diff --git a/core/templates/oa_hash_map.h b/core/templates/oa_hash_map.h
index 2c7c64cd78..025cc30db4 100644
--- a/core/templates/oa_hash_map.h
+++ b/core/templates/oa_hash_map.h
@@ -231,7 +231,7 @@ public:
 	/**
 	 * returns true if the value was found, false otherwise.
 	 *
-	 * if r_data is not nullptr  then the value will be written to the object
+	 * if r_data is not nullptr then the value will be written to the object
 	 * it points to.
 	 */
 	bool lookup(const TKey &p_key, TValue &r_data) const {
@@ -249,7 +249,7 @@ public:
 	/**
 	 * returns true if the value was found, false otherwise.
 	 *
-	 * if r_data is not nullptr  then the value will be written to the object
+	 * if r_data is not nullptr then the value will be written to the object
 	 * it points to.
 	 */
 	TValue *lookup_ptr(const TKey &p_key) const {
diff --git a/core/templates/paged_allocator.h b/core/templates/paged_allocator.h
index 7002034710..dfc885c6eb 100644
--- a/core/templates/paged_allocator.h
+++ b/core/templates/paged_allocator.h
@@ -35,6 +35,8 @@
 #include "core/os/spin_lock.h"
 #include "core/typedefs.h"
 
+#include <type_traits>
+
 template <class T, bool thread_safe = false>
 class PagedAllocator {
 	T **page_pool = nullptr;
@@ -48,7 +50,8 @@ class PagedAllocator {
 	SpinLock spin_lock;
 
 public:
-	T *alloc() {
+	template <class... Args>
+	T *alloc(const Args &&...p_args) {
 		if (thread_safe) {
 			spin_lock.lock();
 		}
@@ -73,7 +76,7 @@ public:
 		if (thread_safe) {
 			spin_lock.unlock();
 		}
-		memnew_placement(alloc, T);
+		memnew_placement(alloc, T(p_args...));
 		return alloc;
 	}
 
@@ -89,8 +92,10 @@ public:
 		allocs_available++;
 	}
 
-	void reset() {
-		ERR_FAIL_COND(allocs_available < pages_allocated * page_size);
+	void reset(bool p_allow_unfreed = false) {
+		if (!p_allow_unfreed || !std::is_trivially_destructible<T>::value) {
+			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]);
diff --git a/core/templates/pair.h b/core/templates/pair.h
index bc1a764694..e30ee8bc56 100644
--- a/core/templates/pair.h
+++ b/core/templates/pair.h
@@ -31,6 +31,8 @@
 #ifndef PAIR_H
 #define PAIR_H
 
+#include "core/typedefs.h"
+
 template <class F, class S>
 struct Pair {
 	F first;
@@ -60,7 +62,43 @@ bool operator!=(const Pair<F, S> &pair, const Pair<F, S> &other) {
 template <class F, class S>
 struct PairSort {
 	bool operator()(const Pair<F, S> &A, const Pair<F, S> &B) const {
-		return A.first < B.first;
+		if (A.first != B.first) {
+			return A.first < B.first;
+		}
+		return A.second < B.second;
+	}
+};
+
+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;
 	}
 };
 
diff --git a/core/templates/pooled_list.h b/core/templates/pooled_list.h
new file mode 100644
index 0000000000..b4a6d2d1dd
--- /dev/null
+++ b/core/templates/pooled_list.h
@@ -0,0 +1,95 @@
+/*************************************************************************/
+/*  pooled_list.h                                                        */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2021 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.                */
+/*************************************************************************/
+
+#pragma once
+
+// 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.
+
+#include "core/templates/local_vector.h"
+
+template <class T, bool force_trivial = false>
+class PooledList {
+	LocalVector<T, uint32_t, force_trivial> list;
+	LocalVector<uint32_t, uint32_t, true> freelist;
+
+	// not all list members are necessarily used
+	int _used_size;
+
+public:
+	PooledList() {
+		_used_size = 0;
+	}
+
+	int estimate_memory_use() const {
+		return (list.size() * sizeof(T)) + (freelist.size() * sizeof(uint32_t));
+	}
+
+	const T &operator[](uint32_t p_index) const {
+		return list[p_index];
+	}
+	T &operator[](uint32_t p_index) {
+		return list[p_index];
+	}
+
+	int size() const { return _used_size; }
+
+	T *request(uint32_t &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);
+		return &list[r_id];
+	}
+	void free(const uint32_t &p_id) {
+		// should not be on free list already
+		CRASH_COND(p_id >= list.size());
+		freelist.push_back(p_id);
+		_used_size--;
+	}
+};
diff --git a/core/templates/rid_owner.h b/core/templates/rid_owner.h
index c4aa93c394..8d139551ef 100644
--- a/core/templates/rid_owner.h
+++ b/core/templates/rid_owner.h
@@ -79,7 +79,7 @@ class RID_Alloc : public RID_AllocBase {
 
 	SpinLock spin_lock;
 
-	_FORCE_INLINE_ RID _allocate_rid(const T *p_initializer) {
+	_FORCE_INLINE_ RID _allocate_rid() {
 		if (THREAD_SAFE) {
 			spin_lock.lock();
 		}
@@ -101,7 +101,7 @@ class RID_Alloc : public RID_AllocBase {
 
 			//initialize
 			for (uint32_t i = 0; i < elements_in_chunk; i++) {
-				//dont initialize chunk
+				// Don't initialize chunk.
 				validator_chunks[chunk_count][i] = 0xFFFFFFFF;
 				free_list_chunks[chunk_count][i] = alloc_count + i;
 			}
@@ -114,11 +114,6 @@ class RID_Alloc : public RID_AllocBase {
 		uint32_t free_chunk = free_index / elements_in_chunk;
 		uint32_t free_element = free_index % elements_in_chunk;
 
-		if (p_initializer) {
-			T *ptr = &chunks[free_chunk][free_element];
-			memnew_placement(ptr, T(*p_initializer));
-		}
-
 		uint32_t validator = (uint32_t)(_gen_id() & 0x7FFFFFFF);
 		uint64_t id = validator;
 		id <<= 32;
@@ -126,9 +121,7 @@ class RID_Alloc : public RID_AllocBase {
 
 		validator_chunks[free_chunk][free_element] = validator;
 
-		if (!p_initializer) {
-			validator_chunks[free_chunk][free_element] |= 0x80000000; //mark uninitialized bit
-		}
+		validator_chunks[free_chunk][free_element] |= 0x80000000; //mark uninitialized bit
 
 		alloc_count++;
 
@@ -140,13 +133,20 @@ class RID_Alloc : public RID_AllocBase {
 	}
 
 public:
+	RID make_rid() {
+		RID rid = _allocate_rid();
+		initialize_rid(rid);
+		return rid;
+	}
 	RID make_rid(const T &p_value) {
-		return _allocate_rid(&p_value);
+		RID rid = _allocate_rid();
+		initialize_rid(rid, p_value);
+		return rid;
 	}
 
 	//allocate but don't initialize, use initialize_rid afterwards
 	RID allocate_rid() {
-		return _allocate_rid(nullptr);
+		return _allocate_rid();
 	}
 
 	_FORCE_INLINE_ T *getornull(const RID &p_rid, bool p_initialize = false) {
@@ -193,7 +193,7 @@ public:
 			if (THREAD_SAFE) {
 				spin_lock.unlock();
 			}
-			if (validator_chunks[idx_chunk][idx_element] & 0x80000000) {
+			if ((validator_chunks[idx_chunk][idx_element] & 0x80000000) && validator_chunks[idx_chunk][idx_element] != 0xFFFFFFFF) {
 				ERR_FAIL_V_MSG(nullptr, "Attempting to use an uninitialized RID");
 			}
 			return nullptr;
@@ -207,6 +207,11 @@ public:
 
 		return ptr;
 	}
+	void initialize_rid(RID p_rid) {
+		T *mem = getornull(p_rid, true);
+		ERR_FAIL_COND(!mem);
+		memnew_placement(mem, T);
+	}
 	void initialize_rid(RID p_rid, const T &p_value) {
 		T *mem = getornull(p_rid, true);
 		ERR_FAIL_COND(!mem);
@@ -333,7 +338,7 @@ public:
 		description = p_descrption;
 	}
 
-	RID_Alloc(uint32_t p_target_chunk_byte_size = 4096) {
+	RID_Alloc(uint32_t p_target_chunk_byte_size = 65536) {
 		elements_in_chunk = sizeof(T) > p_target_chunk_byte_size ? 1 : (p_target_chunk_byte_size / sizeof(T));
 	}
 
@@ -351,6 +356,9 @@ public:
 
 			for (size_t i = 0; i < max_alloc; i++) {
 				uint64_t validator = validator_chunks[i / elements_in_chunk][i % elements_in_chunk];
+				if (validator & 0x80000000) {
+					continue; //uninitialized
+				}
 				if (validator != 0xFFFFFFFF) {
 					chunks[i / elements_in_chunk][i % elements_in_chunk].~T();
 				}
@@ -431,7 +439,7 @@ public:
 		alloc.set_description(p_descrption);
 	}
 
-	RID_PtrOwner(uint32_t p_target_chunk_byte_size = 4096) :
+	RID_PtrOwner(uint32_t p_target_chunk_byte_size = 65536) :
 			alloc(p_target_chunk_byte_size) {}
 };
 
@@ -440,6 +448,9 @@ class RID_Owner {
 	RID_Alloc<T, THREAD_SAFE> alloc;
 
 public:
+	_FORCE_INLINE_ RID make_rid() {
+		return alloc.make_rid();
+	}
 	_FORCE_INLINE_ RID make_rid(const T &p_ptr) {
 		return alloc.make_rid(p_ptr);
 	}
@@ -448,6 +459,10 @@ public:
 		return alloc.allocate_rid();
 	}
 
+	_FORCE_INLINE_ void initialize_rid(RID p_rid) {
+		alloc.initialize_rid(p_rid);
+	}
+
 	_FORCE_INLINE_ void initialize_rid(RID p_rid, const T &p_ptr) {
 		alloc.initialize_rid(p_rid, p_ptr);
 	}
@@ -483,7 +498,7 @@ public:
 	void set_description(const char *p_descrption) {
 		alloc.set_description(p_descrption);
 	}
-	RID_Owner(uint32_t p_target_chunk_byte_size = 4096) :
+	RID_Owner(uint32_t p_target_chunk_byte_size = 65536) :
 			alloc(p_target_chunk_byte_size) {}
 };
 
diff --git a/core/templates/set.h b/core/templates/set.h
index 3036ecf27d..9261d2d3d2 100644
--- a/core/templates/set.h
+++ b/core/templates/set.h
@@ -71,12 +71,94 @@ public:
 		Element *prev() {
 			return _prev;
 		}
+		T &get() {
+			return value;
+		}
 		const T &get() const {
 			return value;
 		};
 		Element() {}
 	};
 
+	typedef T ValueType;
+
+	struct Iterator {
+		_FORCE_INLINE_ T &operator*() const {
+			return E->get();
+		}
+		_FORCE_INLINE_ T *operator->() const { return &E->get(); }
+		_FORCE_INLINE_ Iterator &operator++() {
+			E = E->next();
+			return *this;
+		}
+		_FORCE_INLINE_ Iterator &operator--() {
+			E = E->prev();
+			return *this;
+		}
+
+		_FORCE_INLINE_ bool operator==(const Iterator &b) const { return E == b.E; }
+		_FORCE_INLINE_ bool operator!=(const Iterator &b) const { return E != b.E; }
+
+		Iterator(Element *p_E) { E = p_E; }
+		Iterator() {}
+		Iterator(const Iterator &p_it) { E = p_it.E; }
+
+	private:
+		Element *E = nullptr;
+	};
+
+	struct ConstIterator {
+		_FORCE_INLINE_ const T &operator*() const {
+			return E->get();
+		}
+		_FORCE_INLINE_ const T *operator->() const { return &E->get(); }
+		_FORCE_INLINE_ ConstIterator &operator++() {
+			E = E->next();
+			return *this;
+		}
+		_FORCE_INLINE_ ConstIterator &operator--() {
+			E = E->prev();
+			return *this;
+		}
+
+		_FORCE_INLINE_ bool operator==(const ConstIterator &b) const { return E == b.E; }
+		_FORCE_INLINE_ bool operator!=(const ConstIterator &b) const { return E != b.E; }
+
+		_FORCE_INLINE_ ConstIterator(const Element *p_E) { E = p_E; }
+		_FORCE_INLINE_ ConstIterator() {}
+		_FORCE_INLINE_ ConstIterator(const ConstIterator &p_it) { E = p_it.E; }
+
+	private:
+		const Element *E = nullptr;
+	};
+
+	_FORCE_INLINE_ Iterator begin() {
+		return Iterator(front());
+	}
+	_FORCE_INLINE_ Iterator end() {
+		return Iterator(nullptr);
+	}
+
+#if 0
+	//to use when replacing find()
+	_FORCE_INLINE_ Iterator find(const K &p_key) {
+		return Iterator(find(p_key));
+	}
+#endif
+
+	_FORCE_INLINE_ ConstIterator begin() const {
+		return ConstIterator(front());
+	}
+	_FORCE_INLINE_ ConstIterator end() const {
+		return ConstIterator(nullptr);
+	}
+
+#if 0
+	//to use when replacing find()
+	_FORCE_INLINE_ ConstIterator find(const K &p_key) const {
+		return ConstIterator(find(p_key));
+	}
+#endif
 private:
 	struct _Data {
 		Element *_root = nullptr;
diff --git a/core/templates/thread_work_pool.h b/core/templates/thread_work_pool.h
index 9f7a692cc5..b242648bc8 100644
--- a/core/templates/thread_work_pool.h
+++ b/core/templates/thread_work_pool.h
@@ -105,7 +105,7 @@ public:
 	}
 
 	bool is_done_dispatching() const {
-		ERR_FAIL_COND_V(current_work == nullptr, false);
+		ERR_FAIL_COND_V(current_work == nullptr, true);
 		return index.load(std::memory_order_acquire) >= current_work->max_elements;
 	}
 
diff --git a/core/templates/vector.h b/core/templates/vector.h
index dae8874a87..08cbef6ba4 100644
--- a/core/templates/vector.h
+++ b/core/templates/vector.h
@@ -187,6 +187,70 @@ public:
 		return false;
 	}
 
+	struct Iterator {
+		_FORCE_INLINE_ T &operator*() const {
+			return *elem_ptr;
+		}
+		_FORCE_INLINE_ T *operator->() const { return elem_ptr; }
+		_FORCE_INLINE_ Iterator &operator++() {
+			elem_ptr++;
+			return *this;
+		}
+		_FORCE_INLINE_ Iterator &operator--() {
+			elem_ptr--;
+			return *this;
+		}
+
+		_FORCE_INLINE_ bool operator==(const Iterator &b) const { return elem_ptr == b.elem_ptr; }
+		_FORCE_INLINE_ bool operator!=(const Iterator &b) const { return elem_ptr != b.elem_ptr; }
+
+		Iterator(T *p_ptr) { elem_ptr = p_ptr; }
+		Iterator() {}
+		Iterator(const Iterator &p_it) { elem_ptr = p_it.elem_ptr; }
+
+	private:
+		T *elem_ptr = nullptr;
+	};
+
+	struct ConstIterator {
+		_FORCE_INLINE_ const T &operator*() const {
+			return *elem_ptr;
+		}
+		_FORCE_INLINE_ const T *operator->() const { return elem_ptr; }
+		_FORCE_INLINE_ ConstIterator &operator++() {
+			elem_ptr++;
+			return *this;
+		}
+		_FORCE_INLINE_ ConstIterator &operator--() {
+			elem_ptr--;
+			return *this;
+		}
+
+		_FORCE_INLINE_ bool operator==(const ConstIterator &b) const { return elem_ptr == b.elem_ptr; }
+		_FORCE_INLINE_ bool operator!=(const ConstIterator &b) const { return elem_ptr != b.elem_ptr; }
+
+		ConstIterator(T *p_ptr) { elem_ptr = p_ptr; }
+		ConstIterator() {}
+		ConstIterator(const ConstIterator &p_it) { elem_ptr = p_it.elem_ptr; }
+
+	private:
+		const T *elem_ptr = nullptr;
+	};
+
+	_FORCE_INLINE_ Iterator begin() {
+		return Iterator(ptrw());
+	}
+	_FORCE_INLINE_ Iterator end() {
+		return Iterator(ptrw() + size());
+	}
+
+	_FORCE_INLINE_ ConstIterator begin() const {
+		return ConstIterator(ptr());
+	}
+	_FORCE_INLINE_ ConstIterator end() const {
+		return ConstIterator(ptr() + size());
+	}
+
 	_FORCE_INLINE_ Vector() {}
 	_FORCE_INLINE_ Vector(const Vector &p_from) { _cowdata._ref(p_from._cowdata); }