Merge pull request #40113 from madmiraal/fix-40090

Remove elements from monitored_bodies and monitored_areas as they are processed

2 files changed, 32 insertions, 23 deletions

diff --git a/servers/physics_2d/area_2d_sw.cpp b/servers/physics_2d/area_2d_sw.cpp
index acbbb7e1e8..7485f31afc 100644
--- a/servers/physics_2d/area_2d_sw.cpp
+++ b/servers/physics_2d/area_2d_sw.cpp
@@ -213,9 +213,10 @@ void Area2DSW::call_queries() {
 			return;
 		}
 
-		for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E; E = E->next()) {
-			if (E->get().state == 0) {
-				continue; //nothing happened
+		for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E;) {
+			if (E->get().state == 0) { // Nothing happened
+				E = E->next();
+				continue;
 			}
 
 			res[0] = E->get().state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED;
@@ -224,13 +225,15 @@ void Area2DSW::call_queries() {
 			res[3] = E->key().body_shape;
 			res[4] = E->key().area_shape;
 
+			Map<BodyKey, BodyState>::Element *next = E->next();
+			monitored_bodies.erase(E);
+			E = next;
+
 			Callable::CallError ce;
 			obj->call(monitor_callback_method, (const Variant **)resptr, 5, ce);
 		}
 	}
 
-	monitored_bodies.clear();
-
 	if (area_monitor_callback_id.is_valid() && !monitored_areas.empty()) {
 		Variant res[5];
 		Variant *resptr[5];
@@ -245,9 +248,10 @@ void Area2DSW::call_queries() {
 			return;
 		}
 
-		for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E; E = E->next()) {
-			if (E->get().state == 0) {
-				continue; //nothing happened
+		for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E;) {
+			if (E->get().state == 0) { // Nothing happened
+				E = E->next();
+				continue;
 			}
 
 			res[0] = E->get().state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED;
@@ -256,14 +260,14 @@ void Area2DSW::call_queries() {
 			res[3] = E->key().body_shape;
 			res[4] = E->key().area_shape;
 
+			Map<BodyKey, BodyState>::Element *next = E->next();
+			monitored_areas.erase(E);
+			E = next;
+
 			Callable::CallError ce;
 			obj->call(area_monitor_callback_method, (const Variant **)resptr, 5, ce);
 		}
 	}
-
-	monitored_areas.clear();
-
-	//get_space()->area_remove_from_monitor_query_list(&monitor_query_list);
 }
 
 Area2DSW::Area2DSW() :
diff --git a/servers/physics_3d/area_3d_sw.cpp b/servers/physics_3d/area_3d_sw.cpp
index 98237dd91c..571f1435de 100644
--- a/servers/physics_3d/area_3d_sw.cpp
+++ b/servers/physics_3d/area_3d_sw.cpp
@@ -213,9 +213,10 @@ void Area3DSW::call_queries() {
 			return;
 		}
 
-		for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E; E = E->next()) {
-			if (E->get().state == 0) {
-				continue; //nothing happened
+		for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E;) {
+			if (E->get().state == 0) { // Nothing happened
+				E = E->next();
+				continue;
 			}
 
 			res[0] = E->get().state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED;
@@ -224,13 +225,15 @@ void Area3DSW::call_queries() {
 			res[3] = E->key().body_shape;
 			res[4] = E->key().area_shape;
 
+			Map<BodyKey, BodyState>::Element *next = E->next();
+			monitored_bodies.erase(E);
+			E = next;
+
 			Callable::CallError ce;
 			obj->call(monitor_callback_method, (const Variant **)resptr, 5, ce);
 		}
 	}
 
-	monitored_bodies.clear();
-
 	if (area_monitor_callback_id.is_valid() && !monitored_areas.empty()) {
 		Variant res[5];
 		Variant *resptr[5];
@@ -245,9 +248,10 @@ void Area3DSW::call_queries() {
 			return;
 		}
 
-		for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E; E = E->next()) {
-			if (E->get().state == 0) {
-				continue; //nothing happened
+		for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E;) {
+			if (E->get().state == 0) { // Nothing happened
+				E = E->next();
+				continue;
 			}
 
 			res[0] = E->get().state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED;
@@ -256,13 +260,14 @@ void Area3DSW::call_queries() {
 			res[3] = E->key().body_shape;
 			res[4] = E->key().area_shape;
 
+			Map<BodyKey, BodyState>::Element *next = E->next();
+			monitored_areas.erase(E);
+			E = next;
+
 			Callable::CallError ce;
 			obj->call(area_monitor_callback_method, (const Variant **)resptr, 5, ce);
 		}
 	}
-
-	monitored_areas.clear();
-	//get_space()->area_remove_from_monitor_query_list(&monitor_query_list);
 }
 
 Area3DSW::Area3DSW() :