From a1db6784d6c1d40bb9a41b1ba3d54bd1dac7b911 Mon Sep 17 00:00:00 2001
From: PouleyKetchoupp <pouleyketchoup@gmail.com>
Date: Mon, 12 Apr 2021 20:04:13 -0700
Subject: [PATCH] Fix crashes with CollisionObject debug shapes

MeshInstance added as child nodes for CollisionObject debug shapes can
be invalidated while deleting the collision object (child nodes are
deleted first), which caused accesses to invalid memory in
shape_owner_remove_shape that lead to random crashes.

Also optimized accesses to shapes to avoid copy-on-write on each
iteration.
---
 scene/3d/collision_object.cpp | 27 ++++++++++++++++++++++++++-
 scene/3d/collision_object.h   |  2 ++
 2 files changed, 28 insertions(+), 1 deletion(-)

diff --git a/scene/3d/collision_object.cpp b/scene/3d/collision_object.cpp
index f8c8e1eb1de..5331a060088 100644
--- a/scene/3d/collision_object.cpp
+++ b/scene/3d/collision_object.cpp
@@ -77,6 +77,11 @@ void CollisionObject::_notification(int p_what) {
 				PhysicsServer::get_singleton()->body_set_space(rid, RID());
 
 		} break;
+		case NOTIFICATION_PREDELETE: {
+			if (debug_shape_count > 0) {
+				_clear_debug_shapes();
+			}
+		} break;
 	}
 }
 
@@ -119,11 +124,13 @@ void CollisionObject::_update_debug_shapes() {
 	for (Set<uint32_t>::Element *shapedata_idx = debug_shapes_to_update.front(); shapedata_idx; shapedata_idx = shapedata_idx->next()) {
 		if (shapes.has(shapedata_idx->get())) {
 			ShapeData &shapedata = shapes[shapedata_idx->get()];
+			ShapeData::ShapeBase *shapes = shapedata.shapes.ptrw();
 			for (int i = 0; i < shapedata.shapes.size(); i++) {
-				ShapeData::ShapeBase &s = shapedata.shapes.write[i];
+				ShapeData::ShapeBase &s = shapes[i];
 				if (s.debug_shape) {
 					s.debug_shape->queue_delete();
 					s.debug_shape = nullptr;
+					--debug_shape_count;
 				}
 				if (s.shape.is_null() || shapedata.disabled) {
 					continue;
@@ -137,12 +144,29 @@ void CollisionObject::_update_debug_shapes() {
 
 				mi->force_update_transform();
 				s.debug_shape = mi;
+				++debug_shape_count;
 			}
 		}
 	}
 	debug_shapes_to_update.clear();
 }
 
+void CollisionObject::_clear_debug_shapes() {
+	for (Map<uint32_t, ShapeData>::Element *E = shapes.front(); E; E = E->next()) {
+		ShapeData &shapedata = E->get();
+		ShapeData::ShapeBase *shapes = shapedata.shapes.ptrw();
+		for (int i = 0; i < shapedata.shapes.size(); i++) {
+			ShapeData::ShapeBase &s = shapes[i];
+			if (s.debug_shape) {
+				s.debug_shape->queue_delete();
+				s.debug_shape = nullptr;
+			}
+		}
+	}
+
+	debug_shape_count = 0;
+}
+
 void CollisionObject::_update_shape_data(uint32_t p_owner) {
 	if (is_inside_tree() && get_tree()->is_debugging_collisions_hint() && !Engine::get_singleton()->is_editor_hint()) {
 		if (debug_shapes_to_update.empty()) {
@@ -352,6 +376,7 @@ void CollisionObject::shape_owner_remove_shape(uint32_t p_owner, int p_shape) {
 
 	if (s.debug_shape) {
 		s.debug_shape->queue_delete();
+		--debug_shape_count;
 	}
 
 	shapes[p_owner].shapes.remove(p_shape);
diff --git a/scene/3d/collision_object.h b/scene/3d/collision_object.h
index 8583e99a198..02f5c8bef94 100644
--- a/scene/3d/collision_object.h
+++ b/scene/3d/collision_object.h
@@ -69,6 +69,7 @@ class CollisionObject : public Spatial {
 	bool ray_pickable;
 
 	Set<uint32_t> debug_shapes_to_update;
+	int debug_shape_count = 0;
 
 	void _update_pickable();
 
@@ -85,6 +86,7 @@ protected:
 	virtual void _mouse_exit();
 
 	void _update_debug_shapes();
+	void _clear_debug_shapes();
 
 public:
 	uint32_t create_shape_owner(Object *p_owner);