1 files changed, 20 insertions, 15 deletions

diff --git a/servers/physics_3d/area_3d_sw.cpp b/servers/physics_3d/area_3d_sw.cpp
index 98237dd91c..b6c5b3003c 100644
--- a/servers/physics_3d/area_3d_sw.cpp
+++ b/servers/physics_3d/area_3d_sw.cpp
@@ -5,8 +5,8 @@
 /*                           GODOT ENGINE                                */
 /*                      https://godotengine.org                          */
 /*************************************************************************/
-/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md).   */
+/* 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       */
@@ -199,7 +199,7 @@ void Area3DSW::set_monitorable(bool p_monitorable) {
 }
 
 void Area3DSW::call_queries() {
-	if (monitor_callback_id.is_valid() && !monitored_bodies.empty()) {
+	if (monitor_callback_id.is_valid() && !monitored_bodies.is_empty()) {
 		Variant res[5];
 		Variant *resptr[5];
 		for (int i = 0; i < 5; i++) {
@@ -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,14 +225,16 @@ 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()) {
+	if (area_monitor_callback_id.is_valid() && !monitored_areas.is_empty()) {
 		Variant res[5];
 		Variant *resptr[5];
 		for (int i = 0; i < 5; i++) {
@@ -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() :