Merge pull request #40114 from madmiraal/fix-40090-3.2

[3.2] Remove elements from monitored_bodies and monitored_areas as they are processed

servers/physics/area_sw.cpp

@@ -189,10 +189,12 @@ void AreaSW::call_queries() {
 			return;
 		}
 
-		for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E; E = E->next()) {
+		for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E;) {
 
-			if (E->get().state == 0)
-				continue; //nothing happened
+			if (E->get().state == 0) { // Nothing happened
+				E = E->next();
+				continue;
+			}
 
 			res[0] = E->get().state > 0 ? PhysicsServer::AREA_BODY_ADDED : PhysicsServer::AREA_BODY_REMOVED;
 			res[1] = E->key().rid;
@@ -200,13 +202,15 @@ void AreaSW::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;
+
 			Variant::CallError ce;
 			obj->call(monitor_callback_method, (const Variant **)resptr, 5, ce);
 		}
 	}
 
-	monitored_bodies.clear();
-
 	if (area_monitor_callback_id && !monitored_areas.empty()) {
 
 		Variant res[5];
@@ -221,10 +225,12 @@ void AreaSW::call_queries() {
 			return;
 		}
 
-		for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E; E = E->next()) {
+		for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E;) {
 
-			if (E->get().state == 0)
-				continue; //nothing happened
+			if (E->get().state == 0) { // Nothing happened
+				E = E->next();
+				continue;
+			}
 
 			res[0] = E->get().state > 0 ? PhysicsServer::AREA_BODY_ADDED : PhysicsServer::AREA_BODY_REMOVED;
 			res[1] = E->key().rid;
@@ -232,13 +238,14 @@ void AreaSW::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;
+
 			Variant::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);
 }
 
 AreaSW::AreaSW() :

servers/physics_2d/area_2d_sw.cpp

@@ -189,10 +189,12 @@ void Area2DSW::call_queries() {
 			return;
 		}
 
-		for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E; E = E->next()) {
+		for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E;) {
 
-			if (E->get().state == 0)
-				continue; //nothing happened
+			if (E->get().state == 0) { // Nothing happened
+				E = E->next();
+				continue;
+			}
 
 			res[0] = E->get().state > 0 ? Physics2DServer::AREA_BODY_ADDED : Physics2DServer::AREA_BODY_REMOVED;
 			res[1] = E->key().rid;
@@ -200,13 +202,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;
+
 			Variant::CallError ce;
 			obj->call(monitor_callback_method, (const Variant **)resptr, 5, ce);
 		}
 	}
 
-	monitored_bodies.clear();
-
 	if (area_monitor_callback_id && !monitored_areas.empty()) {
 
 		Variant res[5];
@@ -221,10 +225,12 @@ void Area2DSW::call_queries() {
 			return;
 		}
 
-		for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E; E = E->next()) {
+		for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E;) {
 
-			if (E->get().state == 0)
-				continue; //nothing happened
+			if (E->get().state == 0) { // Nothing happened
+				E = E->next();
+				continue;
+			}
 
 			res[0] = E->get().state > 0 ? Physics2DServer::AREA_BODY_ADDED : Physics2DServer::AREA_BODY_REMOVED;
 			res[1] = E->key().rid;
@@ -232,14 +238,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;
+
 			Variant::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() :