Create CollisionObject3D debug shapes using RS

5 files changed, 106 insertions, 52 deletions

diff --git a/scene/3d/collision_object_3d.cpp b/scene/3d/collision_object_3d.cpp
index 688509a979..9e2912e60f 100644
--- a/scene/3d/collision_object_3d.cpp
+++ b/scene/3d/collision_object_3d.cpp
@@ -31,12 +31,27 @@
 #include "collision_object_3d.h"
 
 #include "core/config/engine.h"
-#include "mesh_instance_3d.h"
 #include "scene/scene_string_names.h"
 #include "servers/physics_server_3d.h"
 
 void CollisionObject3D::_notification(int p_what) {
 	switch (p_what) {
+		case NOTIFICATION_ENTER_TREE: {
+			if (_are_collision_shapes_visible()) {
+				debug_shape_old_transform = get_global_transform();
+				for (Map<uint32_t, ShapeData>::Element *E = shapes.front(); E; E = E->next()) {
+					debug_shapes_to_update.insert(E->key());
+				}
+				_update_debug_shapes();
+			}
+		} break;
+
+		case NOTIFICATION_EXIT_TREE: {
+			if (debug_shapes_count > 0) {
+				_clear_debug_shapes();
+			}
+		} break;
+
 		case NOTIFICATION_ENTER_WORLD: {
 			if (area) {
 				PhysicsServer3D::get_singleton()->area_set_transform(rid, get_global_transform());
@@ -62,6 +77,8 @@ void CollisionObject3D::_notification(int p_what) {
 				PhysicsServer3D::get_singleton()->body_set_state(rid, PhysicsServer3D::BODY_STATE_TRANSFORM, get_global_transform());
 			}
 
+			_on_transform_changed();
+
 		} break;
 		case NOTIFICATION_VISIBILITY_CHANGED: {
 			_update_pickable();
@@ -75,11 +92,6 @@ void CollisionObject3D::_notification(int p_what) {
 			}
 
 		} break;
-		case NOTIFICATION_PREDELETE: {
-			if (debug_shape_count > 0) {
-				_clear_debug_shapes();
-			}
-		} break;
 	}
 }
 
@@ -175,6 +187,33 @@ void CollisionObject3D::_update_pickable() {
 	}
 }
 
+bool CollisionObject3D::_are_collision_shapes_visible() {
+	return is_inside_tree() && get_tree()->is_debugging_collisions_hint() && !Engine::get_singleton()->is_editor_hint();
+}
+
+void CollisionObject3D::_update_shape_data(uint32_t p_owner) {
+	if (_are_collision_shapes_visible()) {
+		if (debug_shapes_to_update.is_empty()) {
+			callable_mp(this, &CollisionObject3D::_update_debug_shapes).call_deferred({}, 0);
+		}
+		debug_shapes_to_update.insert(p_owner);
+	}
+}
+
+void CollisionObject3D::_shape_changed(Ref<Shape3D> p_shape) {
+	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.shape == p_shape && s.debug_shape.is_valid()) {
+				Ref<Mesh> mesh = s.shape->get_debug_mesh();
+				RS::get_singleton()->instance_set_base(s.debug_shape, mesh->get_rid());
+			}
+		}
+	}
+}
+
 void CollisionObject3D::_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())) {
@@ -182,23 +221,30 @@ void CollisionObject3D::_update_debug_shapes() {
 			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;
-				}
 				if (s.shape.is_null() || shapedata.disabled) {
+					if (s.debug_shape.is_valid()) {
+						RS::get_singleton()->free(s.debug_shape);
+						s.debug_shape = RID();
+						--debug_shapes_count;
+					}
 					continue;
 				}
 
+				if (s.debug_shape.is_null()) {
+					s.debug_shape = RS::get_singleton()->instance_create();
+					RS::get_singleton()->instance_set_scenario(s.debug_shape, get_world_3d()->get_scenario());
+
+					if (!s.shape->is_connected("changed", callable_mp(this, &CollisionObject3D::_shape_changed))) {
+						s.shape->connect("changed", callable_mp(this, &CollisionObject3D::_shape_changed),
+								varray(s.shape), CONNECT_DEFERRED);
+					}
+
+					++debug_shapes_count;
+				}
+
 				Ref<Mesh> mesh = s.shape->get_debug_mesh();
-				MeshInstance3D *mi = memnew(MeshInstance3D);
-				mi->set_transform(shapedata.xform);
-				mi->set_mesh(mesh);
-				add_child(mi);
-				mi->force_update_transform();
-				s.debug_shape = mi;
-				++debug_shape_count;
+				RS::get_singleton()->instance_set_base(s.debug_shape, mesh->get_rid());
+				RS::get_singleton()->instance_set_transform(s.debug_shape, get_global_transform() * shapedata.xform);
 			}
 		}
 	}
@@ -211,23 +257,28 @@ void CollisionObject3D::_clear_debug_shapes() {
 		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;
+			if (s.debug_shape.is_valid()) {
+				RS::get_singleton()->free(s.debug_shape);
+				s.debug_shape = RID();
+				if (s.shape.is_valid() && s.shape->is_connected("changed", callable_mp(this, &CollisionObject3D::_update_shape_data))) {
+					s.shape->disconnect("changed", callable_mp(this, &CollisionObject3D::_update_shape_data));
+				}
 			}
 		}
 	}
-
-	debug_shape_count = 0;
+	debug_shapes_count = 0;
 }
 
-void CollisionObject3D::_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.is_empty()) {
-			call_deferred("_update_debug_shapes");
+void CollisionObject3D::_on_transform_changed() {
+	if (debug_shapes_count > 0 && !debug_shape_old_transform.is_equal_approx(get_global_transform())) {
+		debug_shape_old_transform = get_global_transform();
+		for (Map<uint32_t, ShapeData>::Element *E = shapes.front(); E; E = E->next()) {
+			ShapeData &shapedata = E->get();
+			const ShapeData::ShapeBase *shapes = shapedata.shapes.ptr();
+			for (int i = 0; i < shapedata.shapes.size(); i++) {
+				RS::get_singleton()->instance_set_transform(shapes[i].debug_shape, debug_shape_old_transform * shapedata.xform);
+			}
 		}
-		debug_shapes_to_update.insert(p_owner);
 	}
 }
 
@@ -270,8 +321,6 @@ void CollisionObject3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("shape_owner_clear_shapes", "owner_id"), &CollisionObject3D::shape_owner_clear_shapes);
 	ClassDB::bind_method(D_METHOD("shape_find_owner", "shape_index"), &CollisionObject3D::shape_find_owner);
 
-	ClassDB::bind_method(D_METHOD("_update_debug_shapes"), &CollisionObject3D::_update_debug_shapes);
-
 	BIND_VMETHOD(MethodInfo("_input_event", PropertyInfo(Variant::OBJECT, "camera"), PropertyInfo(Variant::OBJECT, "event", PROPERTY_HINT_RESOURCE_TYPE, "InputEvent"), PropertyInfo(Variant::VECTOR3, "click_position"), PropertyInfo(Variant::VECTOR3, "click_normal"), PropertyInfo(Variant::INT, "shape_idx")));
 
 	ADD_SIGNAL(MethodInfo("input_event", PropertyInfo(Variant::OBJECT, "camera", PROPERTY_HINT_RESOURCE_TYPE, "Node"), PropertyInfo(Variant::OBJECT, "event", PROPERTY_HINT_RESOURCE_TYPE, "InputEvent"), PropertyInfo(Variant::VECTOR3, "click_position"), PropertyInfo(Variant::VECTOR3, "click_normal"), PropertyInfo(Variant::INT, "shape_idx")));
@@ -316,7 +365,11 @@ void CollisionObject3D::shape_owner_set_disabled(uint32_t p_owner, bool p_disabl
 	ERR_FAIL_COND(!shapes.has(p_owner));
 
 	ShapeData &sd = shapes[p_owner];
+	if (sd.disabled == p_disabled) {
+		return;
+	}
 	sd.disabled = p_disabled;
+
 	for (int i = 0; i < sd.shapes.size(); i++) {
 		if (area) {
 			PhysicsServer3D::get_singleton()->area_set_shape_disabled(rid, sd.shapes[i].index, p_disabled);
@@ -421,7 +474,7 @@ void CollisionObject3D::shape_owner_remove_shape(uint32_t p_owner, int p_shape)
 	ERR_FAIL_COND(!shapes.has(p_owner));
 	ERR_FAIL_INDEX(p_shape, shapes[p_owner].shapes.size());
 
-	const ShapeData::ShapeBase &s = shapes[p_owner].shapes[p_shape];
+	ShapeData::ShapeBase &s = shapes[p_owner].shapes.write[p_shape];
 	int index_to_remove = s.index;
 
 	if (area) {
@@ -430,8 +483,12 @@ void CollisionObject3D::shape_owner_remove_shape(uint32_t p_owner, int p_shape)
 		PhysicsServer3D::get_singleton()->body_remove_shape(rid, index_to_remove);
 	}
 
-	if (s.debug_shape) {
-		s.debug_shape->queue_delete();
+	if (s.debug_shape.is_valid()) {
+		RS::get_singleton()->free(s.debug_shape);
+		if (s.shape.is_valid() && s.shape->is_connected("changed", callable_mp(this, &CollisionObject3D::_update_shape_data))) {
+			s.shape->disconnect("changed", callable_mp(this, &CollisionObject3D::_update_shape_data));
+		}
+		--debug_shapes_count;
 	}
 
 	shapes[p_owner].shapes.remove(p_shape);
diff --git a/scene/3d/collision_object_3d.h b/scene/3d/collision_object_3d.h
index e3901979d3..80bfcbcf05 100644
--- a/scene/3d/collision_object_3d.h
+++ b/scene/3d/collision_object_3d.h
@@ -48,7 +48,7 @@ class CollisionObject3D : public Node3D {
 		Object *owner = nullptr;
 		Transform xform;
 		struct ShapeBase {
-			Node *debug_shape = nullptr;
+			RID debug_shape;
 			Ref<Shape3D> shape;
 			int index = 0;
 		};
@@ -65,25 +65,30 @@ class CollisionObject3D : public Node3D {
 	bool ray_pickable = true;
 
 	Set<uint32_t> debug_shapes_to_update;
-	int debug_shape_count = 0;
+	int debug_shapes_count = 0;
+	Transform debug_shape_old_transform;
 
 	void _update_pickable();
 
+	bool _are_collision_shapes_visible();
 	void _update_shape_data(uint32_t p_owner);
+	void _shape_changed(Ref<Shape3D> p_shape);
+	void _update_debug_shapes();
+	void _clear_debug_shapes();
 
 protected:
 	CollisionObject3D(RID p_rid, bool p_area);
 
 	void _notification(int p_what);
 	static void _bind_methods();
+
+	void _on_transform_changed();
+
 	friend class Viewport;
 	virtual void _input_event(Node *p_camera, const Ref<InputEvent> &p_input_event, const Vector3 &p_pos, const Vector3 &p_normal, int p_shape);
 	virtual void _mouse_enter();
 	virtual void _mouse_exit();
 
-	void _update_debug_shapes();
-	void _clear_debug_shapes();
-
 public:
 	void set_collision_layer(uint32_t p_layer);
 	uint32_t get_collision_layer() const;
diff --git a/scene/3d/collision_shape_3d.cpp b/scene/3d/collision_shape_3d.cpp
index bec87914c0..70d9cebb83 100644
--- a/scene/3d/collision_shape_3d.cpp
+++ b/scene/3d/collision_shape_3d.cpp
@@ -161,12 +161,10 @@ void CollisionShape3D::set_shape(const Ref<Shape3D> &p_shape) {
 	}
 	if (!shape.is_null()) {
 		shape->unregister_owner(this);
-		shape->disconnect("changed", callable_mp(this, &CollisionShape3D::_shape_changed));
 	}
 	shape = p_shape;
 	if (!shape.is_null()) {
 		shape->register_owner(this);
-		shape->connect("changed", callable_mp(this, &CollisionShape3D::_shape_changed));
 	}
 	update_gizmo();
 	if (parent) {
@@ -176,8 +174,9 @@ void CollisionShape3D::set_shape(const Ref<Shape3D> &p_shape) {
 		}
 	}
 
-	if (is_inside_tree()) {
-		_shape_changed();
+	if (is_inside_tree() && parent) {
+		// If this is a heightfield shape our center may have changed
+		_update_in_shape_owner(true);
 	}
 	update_configuration_warnings();
 }
@@ -209,10 +208,3 @@ CollisionShape3D::~CollisionShape3D() {
 	}
 	//RenderingServer::get_singleton()->free(indicator);
 }
-
-void CollisionShape3D::_shape_changed() {
-	// If this is a heightfield shape our center may have changed
-	if (parent) {
-		_update_in_shape_owner(true);
-	}
-}
diff --git a/scene/3d/collision_shape_3d.h b/scene/3d/collision_shape_3d.h
index 56a4ae3039..f69c1e38eb 100644
--- a/scene/3d/collision_shape_3d.h
+++ b/scene/3d/collision_shape_3d.h
@@ -47,8 +47,6 @@ class CollisionShape3D : public Node3D {
 	bool disabled = false;
 
 protected:
-	void _shape_changed();
-
 	void _update_in_shape_owner(bool p_xform_only = false);
 
 protected:
diff --git a/scene/3d/physics_body_3d.cpp b/scene/3d/physics_body_3d.cpp
index 93d3e946fd..dd1a797568 100644
--- a/scene/3d/physics_body_3d.cpp
+++ b/scene/3d/physics_body_3d.cpp
@@ -292,6 +292,7 @@ void RigidBody3D::_direct_state_changed(Object *p_state) {
 		get_script_instance()->call("_integrate_forces", state);
 	}
 	set_ignore_transform_notification(false);
+	_on_transform_changed();
 
 	if (contact_monitor) {
 		contact_monitor->locked = true;
@@ -1985,6 +1986,7 @@ void PhysicalBone3D::_direct_state_changed(Object *p_state) {
 	set_ignore_transform_notification(true);
 	set_global_transform(global_transform);
 	set_ignore_transform_notification(false);
+	_on_transform_changed();
 
 	// Update skeleton
 	if (parent_skeleton) {