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

@@ -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);

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);