From 8e4641bc8883294621e92ffc8ed1ec3496a5912d Mon Sep 17 00:00:00 2001 From: Henry Conklin Date: Sat, 13 Feb 2021 13:31:50 -0600 Subject: [PATCH] Optimize BroadPhase2dHashGrid to not pair elements on different layers Clean up logic in _check_motion Closes 45824 --- .../physics_2d/broad_phase_2d_hash_grid.cpp | 106 +++++++++--------- servers/physics_2d/broad_phase_2d_hash_grid.h | 11 +- 2 files changed, 64 insertions(+), 53 deletions(-) diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.cpp b/servers/physics_2d/broad_phase_2d_hash_grid.cpp index be73d908dee..0df41946e0a 100644 --- a/servers/physics_2d/broad_phase_2d_hash_grid.cpp +++ b/servers/physics_2d/broad_phase_2d_hash_grid.cpp @@ -35,7 +35,12 @@ #define LARGE_ELEMENT_FI 1.01239812 void BroadPhase2DHashGrid::_pair_attempt(Element *p_elem, Element *p_with) { - + if (p_elem->owner == p_with->owner) { + return; + } + if (!_test_collision_mask(p_elem->collision_mask, p_elem->collision_layer, p_with->collision_mask, p_with->collision_layer)) { + return; + } Map::Element *E = p_elem->paired.find(p_with); ERR_FAIL_COND(p_elem->_static && p_with->_static); @@ -51,7 +56,12 @@ void BroadPhase2DHashGrid::_pair_attempt(Element *p_elem, Element *p_with) { } void BroadPhase2DHashGrid::_unpair_attempt(Element *p_elem, Element *p_with) { - + if (p_elem->owner == p_with->owner) { + return; + } + if (!_test_collision_mask(p_elem->collision_mask, p_elem->collision_layer, p_with->collision_mask, p_with->collision_layer)) { + return; + } Map::Element *E = p_elem->paired.find(p_with); ERR_FAIL_COND(!E); //this should really be paired.. @@ -80,36 +90,32 @@ void BroadPhase2DHashGrid::_check_motion(Element *p_elem) { bool physical_collision = p_elem->aabb.intersects(E->key()->aabb); bool logical_collision = p_elem->owner->test_collision_mask(E->key()->owner); - if (physical_collision) { - if (!E->get()->colliding || (logical_collision && !E->get()->ud && pair_callback)) { + if (physical_collision && logical_collision) { + if (!E->get()->colliding && pair_callback) { E->get()->ud = pair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, pair_userdata); - } else if (E->get()->colliding && !logical_collision && E->get()->ud && unpair_callback) { - unpair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, E->get()->ud, unpair_userdata); - E->get()->ud = nullptr; } E->get()->colliding = true; - } else { // No physcial_collision + } else { // No collision if (E->get()->colliding && unpair_callback) { unpair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, E->get()->ud, unpair_userdata); + E->get()->ud = nullptr; } E->get()->colliding = false; } } } -void BroadPhase2DHashGrid::_enter_grid(Element *p_elem, const Rect2 &p_rect, bool p_static) { - +void BroadPhase2DHashGrid::_enter_grid(Element *p_elem, const Rect2 &p_rect, bool p_static, bool p_force_enter) { Vector2 sz = (p_rect.size / cell_size * LARGE_ELEMENT_FI); //use magic number to avoid floating point issues if (sz.width * sz.height > large_object_min_surface) { //large object, do not use grid, must check against all elements for (Map::Element *E = element_map.front(); E; E = E->next()) { - if (E->key() == p_elem->self) + if (E->key() == p_elem->self) { continue; // do not pair against itself - if (E->get().owner == p_elem->owner) + } + if (E->get()._static && p_static) { continue; - if (E->get()._static && p_static) - continue; - + } _pair_attempt(p_elem, &E->get()); } @@ -140,7 +146,7 @@ void BroadPhase2DHashGrid::_enter_grid(Element *p_elem, const Rect2 &p_rect, boo pb = pb->next; } - bool entered = false; + bool entered = p_force_enter; if (!pb) { //does not exist, create! @@ -164,18 +170,12 @@ void BroadPhase2DHashGrid::_enter_grid(Element *p_elem, const Rect2 &p_rect, boo if (entered) { for (Map::Element *E = pb->object_set.front(); E; E = E->next()) { - - if (E->key()->owner == p_elem->owner) - continue; _pair_attempt(p_elem, E->key()); } if (!p_static) { for (Map::Element *E = pb->static_object_set.front(); E; E = E->next()) { - - if (E->key()->owner == p_elem->owner) - continue; _pair_attempt(p_elem, E->key()); } } @@ -186,20 +186,17 @@ void BroadPhase2DHashGrid::_enter_grid(Element *p_elem, const Rect2 &p_rect, boo //pair separatedly with large elements for (Map::Element *E = large_elements.front(); E; E = E->next()) { - - if (E->key() == p_elem) + if (E->key() == p_elem) { continue; // do not pair against itself - if (E->key()->owner == p_elem->owner) + } + if (E->key()->_static && p_static) { continue; - if (E->key()->_static && p_static) - continue; - + } _pair_attempt(E->key(), p_elem); } } -void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool p_static) { - +void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool p_static, bool p_force_exit) { Vector2 sz = (p_rect.size / cell_size * LARGE_ELEMENT_FI); if (sz.width * sz.height > large_object_min_surface) { @@ -242,7 +239,7 @@ void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool ERR_CONTINUE(!pb); //should exist!! - bool exited = false; + bool exited = p_force_exit; if (p_static) { if (pb->static_object_set[p_elem].dec() == 0) { @@ -261,18 +258,12 @@ void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool if (exited) { for (Map::Element *E = pb->object_set.front(); E; E = E->next()) { - - if (E->key()->owner == p_elem->owner) - continue; _unpair_attempt(p_elem, E->key()); } if (!p_static) { for (Map::Element *E = pb->static_object_set.front(); E; E = E->next()) { - - if (E->key()->owner == p_elem->owner) - continue; _unpair_attempt(p_elem, E->key()); } } @@ -305,13 +296,12 @@ void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool } for (Map::Element *E = large_elements.front(); E; E = E->next()) { - if (E->key() == p_elem) + if (E->key() == p_elem) { continue; // do not pair against itself - if (E->key()->owner == p_elem->owner) + } + if (E->key()->_static && p_static) { continue; - if (E->key()->_static && p_static) - continue; - + } //unpair from large elements _unpair_attempt(p_elem, E->key()); } @@ -324,6 +314,8 @@ BroadPhase2DHashGrid::ID BroadPhase2DHashGrid::create(CollisionObject2DSW *p_obj Element e; e.owner = p_object; e._static = false; + e.collision_mask = p_object->get_collision_mask(); + e.collision_layer = p_object->get_collision_layer(); e.subindex = p_subindex; e.self = current; e.pass = 0; @@ -338,17 +330,27 @@ void BroadPhase2DHashGrid::move(ID p_id, const Rect2 &p_aabb) { ERR_FAIL_COND(!E); Element &e = E->get(); + bool layer_changed = e.collision_mask != e.owner->get_collision_mask() || e.collision_layer != e.owner->get_collision_layer(); - if (p_aabb != e.aabb) { - + if (p_aabb != e.aabb || layer_changed) { + uint32_t old_mask = e.collision_mask; + uint32_t old_layer = e.collision_layer; if (p_aabb != Rect2()) { + e.collision_mask = e.owner->get_collision_mask(); + e.collision_layer = e.owner->get_collision_layer(); - _enter_grid(&e, p_aabb, e._static); + _enter_grid(&e, p_aabb, e._static, layer_changed); } if (e.aabb != Rect2()) { + // Need _exit_grid to remove from cells based on the old layer values. + e.collision_mask = old_mask; + e.collision_layer = old_layer; - _exit_grid(&e, e.aabb, e._static); + _exit_grid(&e, e.aabb, e._static, layer_changed); + + e.collision_mask = e.owner->get_collision_mask(); + e.collision_layer = e.owner->get_collision_layer(); } e.aabb = p_aabb; @@ -367,13 +369,14 @@ void BroadPhase2DHashGrid::set_static(ID p_id, bool p_static) { if (e._static == p_static) return; - if (e.aabb != Rect2()) - _exit_grid(&e, e.aabb, e._static); + if (e.aabb != Rect2()) { + _exit_grid(&e, e.aabb, e._static, false); + } e._static = p_static; if (e.aabb != Rect2()) { - _enter_grid(&e, e.aabb, e._static); + _enter_grid(&e, e.aabb, e._static, false); _check_motion(&e); } } @@ -384,8 +387,9 @@ void BroadPhase2DHashGrid::remove(ID p_id) { Element &e = E->get(); - if (e.aabb != Rect2()) - _exit_grid(&e, e.aabb, e._static); + if (e.aabb != Rect2()) { + _exit_grid(&e, e.aabb, e._static, false); + } element_map.erase(p_id); } diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.h b/servers/physics_2d/broad_phase_2d_hash_grid.h index 29e9ccee0a1..5510ff92342 100644 --- a/servers/physics_2d/broad_phase_2d_hash_grid.h +++ b/servers/physics_2d/broad_phase_2d_hash_grid.h @@ -54,6 +54,9 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW { CollisionObject2DSW *owner; bool _static; Rect2 aabb; + // Owner's collision_mask/layer, used to detect changes in layers. + uint32_t collision_mask; + uint32_t collision_layer; int subindex; uint64_t pass; Map paired; @@ -120,8 +123,12 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW { UnpairCallback unpair_callback; void *unpair_userdata; - void _enter_grid(Element *p_elem, const Rect2 &p_rect, bool p_static); - void _exit_grid(Element *p_elem, const Rect2 &p_rect, bool p_static); + static _FORCE_INLINE_ bool _test_collision_mask(uint32_t p_mask1, uint32_t p_layer1, uint32_t p_mask2, uint32_t p_layer2) { + return p_mask1 & p_layer2 || p_mask2 & p_layer1; + } + + void _enter_grid(Element *p_elem, const Rect2 &p_rect, bool p_static, bool p_force_enter); + void _exit_grid(Element *p_elem, const Rect2 &p_rect, bool p_static, bool p_force_exit); template _FORCE_INLINE_ void _cull(const Point2i p_cell, const Rect2 &p_aabb, const Point2 &p_from, const Point2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices, int &index);