From 76e6be466cf4d00e6eb0f4ca47170aa4214f5a85 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20T=2E=20Listwon?= Date: Sun, 18 Apr 2021 14:20:10 +0200 Subject: [PATCH] Fix Multi-Threaded Physics 2D crashes --- core/map_mt.h | 702 ++++++++++++++++++++++++++++++ core/self_list.h | 5 + servers/physics_2d/area_2d_sw.cpp | 12 +- servers/physics_2d/area_2d_sw.h | 5 +- 4 files changed, 716 insertions(+), 8 deletions(-) create mode 100644 core/map_mt.h diff --git a/core/map_mt.h b/core/map_mt.h new file mode 100644 index 000000000000..1d2f8f379708 --- /dev/null +++ b/core/map_mt.h @@ -0,0 +1,702 @@ +/*************************************************************************/ +/* map_mt.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 MAP_MT_H +#define MAP_MT_H + +#include "core/error_macros.h" +#include "core/os/memory.h" +#include "core/os/mutex.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 A = DefaultAllocator> +class MapMT { + + enum Color { + RED, + BLACK + }; + struct _Data; + +public: + class Element { + + private: + friend class MapMT; + 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 = NULL; + left = NULL; + parent = NULL; + _next = NULL; + _prev = NULL; + }; + }; + +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 = NULL; + 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(_root); + _root = NULL; + } + } + + ~_Data() { + + _free_root(); + +#ifdef GLOBALNIL_DISABLED + memdelete_allocator(_nil); +#endif + } + }; + + _Data _data; + Mutex mutex; + + 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 NULL; // 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 NULL; // 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 NULL; + } + + Element *_find_closest(const K &p_key) const { + + Element *node = _data._root->left; + Element *prev = NULL; + 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 == NULL) + return NULL; // tree empty + + if (less(p_key, prev->_key)) + prev = prev->_prev; + + return prev; + } + + inline 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(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(p_element); + } + + void _copy_from(const MapMT &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 { + + MutexLock lock(mutex); + if (!_data._root) + return NULL; + + const Element *res = _find(p_key); + return res; + } + + Element *find(const K &p_key) { + + MutexLock lock(mutex); + if (!_data._root) + return NULL; + + Element *res = _find(p_key); + return res; + } + + const Element *find_closest(const K &p_key) const { + + MutexLock lock(mutex); + if (!_data._root) + return NULL; + + const Element *res = _find_closest(p_key); + return res; + } + + Element *find_closest(const K &p_key) { + + MutexLock lock(mutex); + if (!_data._root) + return NULL; + + Element *res = _find_closest(p_key); + return res; + } + + bool has(const K &p_key) const { + + MutexLock lock(mutex); + return find(p_key) != NULL; + } + + Element *insert(const K &p_key, const V &p_value) { + + MutexLock lock(mutex); + if (!_data._root) + _data._create_root(); + return _insert(p_key, p_value); + } + + void erase(Element *p_element) { + + MutexLock lock(mutex); + 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) { + + MutexLock lock(mutex); + 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 { + + MutexLock lock(mutex); + CRASH_COND(!_data._root); + const Element *e = find(p_key); + CRASH_COND(!e); + return e->_value; + } + + V &operator[](const K &p_key) { + + MutexLock lock(mutex); + if (!_data._root) + _data._create_root(); + + Element *e = find(p_key); + if (!e) + e = insert(p_key, V()); + + return e->_value; + } + + Element *front() const { + + MutexLock lock(mutex); + if (!_data._root) + return NULL; + + Element *e = _data._root->left; + if (e == _data._nil) + return NULL; + + while (e->left != _data._nil) + e = e->left; + + return e; + } + + Element *back() const { + + MutexLock lock(mutex); + if (!_data._root) + return NULL; + + Element *e = _data._root->left; + if (e == _data._nil) + return NULL; + + 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 { + MutexLock lock(mutex); + // 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() { + + MutexLock lock(mutex); + if (!_data._root) + return; + + _cleanup_tree(_data._root->left); + _data._root->left = _data._nil; + _data.size_cache = 0; + _data._free_root(); + } + + void operator=(const MapMT &p_map) { + + MutexLock lock(mutex); + _copy_from(p_map); + } + + MapMT(const MapMT &p_map) { + + MutexLock lock(mutex); + _copy_from(p_map); + } + + _FORCE_INLINE_ MapMT() { + } + + ~MapMT() { + + clear(); + } +}; + +#endif diff --git a/core/self_list.h b/core/self_list.h index e361704fb9a5..9789abafad7d 100644 --- a/core/self_list.h +++ b/core/self_list.h @@ -32,6 +32,7 @@ #define SELF_LIST_H #include "core/error_macros.h" +#include "core/os/mutex.h" #include "core/typedefs.h" template @@ -41,10 +42,12 @@ class SelfList { SelfList *_first; SelfList *_last; + Mutex mutex; public: void add(SelfList *p_elem) { + MutexLock lock(mutex); ERR_FAIL_COND(p_elem->_root); p_elem->_root = this; @@ -63,6 +66,7 @@ class SelfList { void add_last(SelfList *p_elem) { + MutexLock lock(mutex); ERR_FAIL_COND(p_elem->_root); p_elem->_root = this; @@ -81,6 +85,7 @@ class SelfList { void remove(SelfList *p_elem) { + MutexLock lock(mutex); ERR_FAIL_COND(p_elem->_root != this); if (p_elem->_next) { p_elem->_next->_prev = p_elem->_prev; diff --git a/servers/physics_2d/area_2d_sw.cpp b/servers/physics_2d/area_2d_sw.cpp index 649814814b0f..88b22956782f 100644 --- a/servers/physics_2d/area_2d_sw.cpp +++ b/servers/physics_2d/area_2d_sw.cpp @@ -189,10 +189,10 @@ void Area2DSW::call_queries() { return; } - for (Map::Element *E = monitored_bodies.front(); E;) { + for (MapMT::Element *E = monitored_bodies.front(); E;) { if (E->get().state == 0) { // Nothing happened - Map::Element *next = E->next(); + MapMT::Element *next = E->next(); monitored_bodies.erase(E); E = next; continue; @@ -204,7 +204,7 @@ void Area2DSW::call_queries() { res[3] = E->key().body_shape; res[4] = E->key().area_shape; - Map::Element *next = E->next(); + MapMT::Element *next = E->next(); monitored_bodies.erase(E); E = next; @@ -227,10 +227,10 @@ void Area2DSW::call_queries() { return; } - for (Map::Element *E = monitored_areas.front(); E;) { + for (MapMT::Element *E = monitored_areas.front(); E;) { if (E->get().state == 0) { // Nothing happened - Map::Element *next = E->next(); + MapMT::Element *next = E->next(); monitored_areas.erase(E); E = next; continue; @@ -242,7 +242,7 @@ void Area2DSW::call_queries() { res[3] = E->key().body_shape; res[4] = E->key().area_shape; - Map::Element *next = E->next(); + MapMT::Element *next = E->next(); monitored_areas.erase(E); E = next; diff --git a/servers/physics_2d/area_2d_sw.h b/servers/physics_2d/area_2d_sw.h index 2bb16a685f5c..aba2ab1dc62f 100644 --- a/servers/physics_2d/area_2d_sw.h +++ b/servers/physics_2d/area_2d_sw.h @@ -32,6 +32,7 @@ #define AREA_2D_SW_H #include "collision_object_2d_sw.h" +#include "core/map_mt.h" #include "core/self_list.h" #include "servers/physics_2d_server.h" //#include "servers/physics/query_sw.h" @@ -95,8 +96,8 @@ class Area2DSW : public CollisionObject2DSW { _FORCE_INLINE_ BodyState() { state = 0; } }; - Map monitored_bodies; - Map monitored_areas; + MapMT monitored_bodies; + MapMT monitored_areas; //virtual void shape_changed_notify(Shape2DSW *p_shape); //virtual void shape_deleted_notify(Shape2DSW *p_shape);