diff --git a/core/templates/local_vector.h b/core/templates/local_vector.h index 8cafefcb963..3992954743e 100644 --- a/core/templates/local_vector.h +++ b/core/templates/local_vector.h @@ -103,6 +103,15 @@ public: return false; } + bool erase_unordered(const T &p_val) { + int64_t idx = find(p_val); + if (idx >= 0) { + remove_at_unordered(idx); + return true; + } + return false; + } + U erase_multiple_unordered(const T &p_val) { U from = 0; U occurrences = 0; diff --git a/modules/navigation_2d/nav_map_2d.cpp b/modules/navigation_2d/nav_map_2d.cpp index 285462e044c..d417b2f911e 100644 --- a/modules/navigation_2d/nav_map_2d.cpp +++ b/modules/navigation_2d/nav_map_2d.cpp @@ -192,9 +192,7 @@ void NavMap2D::add_region(NavRegion2D *p_region) { } void NavMap2D::remove_region(NavRegion2D *p_region) { - int64_t region_index = regions.find(p_region); - if (region_index >= 0) { - regions.remove_at_unordered(region_index); + if (regions.erase_unordered(p_region)) { iteration_dirty = true; } } @@ -205,9 +203,7 @@ void NavMap2D::add_link(NavLink2D *p_link) { } void NavMap2D::remove_link(NavLink2D *p_link) { - int64_t link_index = links.find(p_link); - if (link_index >= 0) { - links.remove_at_unordered(link_index); + if (links.erase_unordered(p_link)) { iteration_dirty = true; } } @@ -225,9 +221,7 @@ void NavMap2D::add_agent(NavAgent2D *p_agent) { void NavMap2D::remove_agent(NavAgent2D *p_agent) { remove_agent_as_controlled(p_agent); - int64_t agent_index = agents.find(p_agent); - if (agent_index >= 0) { - agents.remove_at_unordered(agent_index); + if (agents.erase_unordered(p_agent)) { agents_dirty = true; } } @@ -249,9 +243,7 @@ void NavMap2D::add_obstacle(NavObstacle2D *p_obstacle) { } void NavMap2D::remove_obstacle(NavObstacle2D *p_obstacle) { - int64_t obstacle_index = obstacles.find(p_obstacle); - if (obstacle_index >= 0) { - obstacles.remove_at_unordered(obstacle_index); + if (obstacles.erase_unordered(p_obstacle)) { obstacles_dirty = true; } } @@ -272,9 +264,7 @@ void NavMap2D::set_agent_as_controlled(NavAgent2D *p_agent) { } void NavMap2D::remove_agent_as_controlled(NavAgent2D *p_agent) { - int64_t agent_index = active_avoidance_agents.find(p_agent); - if (agent_index >= 0) { - active_avoidance_agents.remove_at_unordered(agent_index); + if (active_avoidance_agents.erase_unordered(p_agent)) { agents_dirty = true; } } diff --git a/modules/navigation_3d/nav_map_3d.cpp b/modules/navigation_3d/nav_map_3d.cpp index 1522d7389e7..74393238177 100644 --- a/modules/navigation_3d/nav_map_3d.cpp +++ b/modules/navigation_3d/nav_map_3d.cpp @@ -235,9 +235,7 @@ void NavMap3D::add_region(NavRegion3D *p_region) { } void NavMap3D::remove_region(NavRegion3D *p_region) { - int64_t region_index = regions.find(p_region); - if (region_index >= 0) { - regions.remove_at_unordered(region_index); + if (regions.erase_unordered(p_region)) { iteration_dirty = true; } } @@ -248,9 +246,7 @@ void NavMap3D::add_link(NavLink3D *p_link) { } void NavMap3D::remove_link(NavLink3D *p_link) { - int64_t link_index = links.find(p_link); - if (link_index >= 0) { - links.remove_at_unordered(link_index); + if (links.erase_unordered(p_link)) { iteration_dirty = true; } } @@ -268,9 +264,7 @@ void NavMap3D::add_agent(NavAgent3D *agent) { void NavMap3D::remove_agent(NavAgent3D *agent) { remove_agent_as_controlled(agent); - int64_t agent_index = agents.find(agent); - if (agent_index >= 0) { - agents.remove_at_unordered(agent_index); + if (agents.erase_unordered(agent)) { agents_dirty = true; } } @@ -292,9 +286,7 @@ void NavMap3D::add_obstacle(NavObstacle3D *obstacle) { } void NavMap3D::remove_obstacle(NavObstacle3D *obstacle) { - int64_t obstacle_index = obstacles.find(obstacle); - if (obstacle_index >= 0) { - obstacles.remove_at_unordered(obstacle_index); + if (obstacles.erase_unordered(obstacle)) { obstacles_dirty = true; } } @@ -323,14 +315,10 @@ void NavMap3D::set_agent_as_controlled(NavAgent3D *agent) { } void NavMap3D::remove_agent_as_controlled(NavAgent3D *agent) { - int64_t agent_3d_index = active_3d_avoidance_agents.find(agent); - if (agent_3d_index >= 0) { - active_3d_avoidance_agents.remove_at_unordered(agent_3d_index); + if (active_3d_avoidance_agents.erase_unordered(agent)) { agents_dirty = true; } - int64_t agent_2d_index = active_2d_avoidance_agents.find(agent); - if (agent_2d_index >= 0) { - active_2d_avoidance_agents.remove_at_unordered(agent_2d_index); + if (active_2d_avoidance_agents.erase_unordered(agent)) { agents_dirty = true; } } diff --git a/tests/core/templates/test_local_vector.h b/tests/core/templates/test_local_vector.h index 40f44381a12..dc56a484fe9 100644 --- a/tests/core/templates/test_local_vector.h +++ b/tests/core/templates/test_local_vector.h @@ -178,6 +178,23 @@ TEST_CASE("[LocalVector] Remove Unordered.") { CHECK(vector.size() == 0); } +TEST_CASE("[LocalVector] Erase Unordered.") { + LocalVector vector; + vector.push_back(1); + vector.push_back(3); + vector.push_back(0); + vector.push_back(2); + vector.push_back(4); + + CHECK(vector.find(1) == 0); + + vector.erase_unordered(1); + + CHECK(vector.find(1) == -1); + CHECK(vector.size() == 4); + CHECK(vector[0] == 4); +} + TEST_CASE("[LocalVector] Erase.") { LocalVector vector; vector.push_back(1);