Merge pull request #90644 from BattyBovine/cs3d_debug_colour

Add CollisionShape3D custom debug colors

doc/classes/CollisionShape3D.xml

@@ -29,6 +29,12 @@
 		</method>
 	</methods>
 	<members>
+		<member name="debug_color" type="Color" setter="set_debug_color" getter="get_debug_color" default="Color(0, 0, 0, 0)">
+			The collision shape color that is displayed in the editor, or in the running project if [b]Debug &gt; Visible Collision Shapes[/b] is checked at the top of the editor. If this is reset to its default value of [code]Color(0, 0, 0, 0)[/code], the value of [member ProjectSettings.debug/shapes/collision/shape_color] will be used instead.
+		</member>
+		<member name="debug_fill" type="bool" setter="set_enable_debug_fill" getter="get_enable_debug_fill" default="true">
+			If [code]true[/code], when the shape is displayed, it will show a solid fill color in addition to its wireframe.
+		</member>
 		<member name="disabled" type="bool" setter="set_disabled" getter="is_disabled" default="false" keywords="enabled">
 			A disabled collision shape has no effect in the world.
 		</member>

editor/plugins/gizmos/collision_shape_3d_gizmo_plugin.cpp

@@ -49,17 +49,47 @@
 
 CollisionShape3DGizmoPlugin::CollisionShape3DGizmoPlugin() {
 	helper.instantiate();
-	const Color gizmo_color = SceneTree::get_singleton()->get_debug_collisions_color();
-	create_material("shape_material", gizmo_color);
-	const float gizmo_value = gizmo_color.get_v();
-	const Color gizmo_color_disabled = Color(gizmo_value, gizmo_value, gizmo_value, 0.65);
-	create_material("shape_material_disabled", gizmo_color_disabled);
+
+	create_collision_material("shape_material", 2.0);
+	create_collision_material("shape_material_arraymesh", 0.0625);
+
+	create_collision_material("shape_material_disabled", 0.0625);
+	create_collision_material("shape_material_arraymesh_disabled", 0.015625);
+
 	create_handle_material("handles");
 }
 
 CollisionShape3DGizmoPlugin::~CollisionShape3DGizmoPlugin() {
 }
 
+void CollisionShape3DGizmoPlugin::create_collision_material(const String &p_name, float p_alpha) {
+	Vector<Ref<StandardMaterial3D>> mats;
+
+	const Color collision_color(1.0, 1.0, 1.0, p_alpha);
+
+	for (int i = 0; i < 4; i++) {
+		bool instantiated = i < 2;
+
+		Ref<StandardMaterial3D> material = memnew(StandardMaterial3D);
+
+		Color color = collision_color;
+		color.a *= instantiated ? 0.25 : 1.0;
+
+		material->set_albedo(color);
+		material->set_shading_mode(StandardMaterial3D::SHADING_MODE_UNSHADED);
+		material->set_transparency(StandardMaterial3D::TRANSPARENCY_ALPHA);
+		material->set_render_priority(StandardMaterial3D::RENDER_PRIORITY_MIN + 1);
+		material->set_cull_mode(StandardMaterial3D::CULL_BACK);
+		material->set_flag(StandardMaterial3D::FLAG_DISABLE_FOG, true);
+		material->set_flag(StandardMaterial3D::FLAG_ALBEDO_FROM_VERTEX_COLOR, true);
+		material->set_flag(StandardMaterial3D::FLAG_SRGB_VERTEX_COLOR, true);
+
+		mats.push_back(material);
+	}
+
+	materials[p_name] = mats;
+}
+
 bool CollisionShape3DGizmoPlugin::has_gizmo(Node3D *p_spatial) {
 	return Object::cast_to<CollisionShape3D>(p_spatial) != nullptr;
 }
@@ -311,9 +341,20 @@ void CollisionShape3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
 		return;
 	}
 
-	const Ref<Material> material =
+	const Ref<StandardMaterial3D> material =
 			get_material(!cs->is_disabled() ? "shape_material" : "shape_material_disabled", p_gizmo);
-	Ref<Material> handles_material = get_material("handles");
+	const Ref<StandardMaterial3D> material_arraymesh =
+			get_material(!cs->is_disabled() ? "shape_material_arraymesh" : "shape_material_arraymesh_disabled", p_gizmo);
+	const Ref<Material> handles_material = get_material("handles");
+
+	const Color collision_color = cs->is_disabled() ? Color(1.0, 1.0, 1.0, 0.75) : cs->get_debug_color();
+
+	if (cs->get_debug_fill_enabled()) {
+		Ref<ArrayMesh> array_mesh = s->get_debug_arraymesh_faces(collision_color);
+		if (array_mesh.is_valid()) {
+			p_gizmo->add_mesh(array_mesh, material_arraymesh);
+		}
+	}
 
 	if (Object::cast_to<SphereShape3D>(*s)) {
 		Ref<SphereShape3D> sp = s;
@@ -351,7 +392,7 @@ void CollisionShape3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
 			collision_segments.push_back(Vector3(b.x, b.y, 0));
 		}
 
-		p_gizmo->add_lines(points, material);
+		p_gizmo->add_lines(points, material, false, collision_color);
 		p_gizmo->add_collision_segments(collision_segments);
 		Vector<Vector3> handles;
 		handles.push_back(Vector3(r, 0, 0));
@@ -374,7 +415,7 @@ void CollisionShape3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
 
 		const Vector<Vector3> handles = helper->box_get_handles(bs->get_size());
 
-		p_gizmo->add_lines(lines, material);
+		p_gizmo->add_lines(lines, material, false, collision_color);
 		p_gizmo->add_collision_segments(lines);
 		p_gizmo->add_handles(handles, handles_material);
 	}
@@ -412,7 +453,7 @@ void CollisionShape3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
 			points.push_back(Vector3(b.y, b.x, 0) + dud);
 		}
 
-		p_gizmo->add_lines(points, material);
+		p_gizmo->add_lines(points, material, false, collision_color);
 
 		Vector<Vector3> collision_segments;
 
@@ -476,7 +517,7 @@ void CollisionShape3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
 			}
 		}
 
-		p_gizmo->add_lines(points, material);
+		p_gizmo->add_lines(points, material, false, collision_color);
 
 		Vector<Vector3> collision_segments;
 
@@ -531,7 +572,7 @@ void CollisionShape3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
 			p.normal * p.d + p.normal * 3
 		};
 
-		p_gizmo->add_lines(points, material);
+		p_gizmo->add_lines(points, material, false, collision_color);
 		p_gizmo->add_collision_segments(points);
 	}
 
@@ -549,7 +590,7 @@ void CollisionShape3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
 					lines.write[i * 2 + 0] = md.vertices[md.edges[i].vertex_a];
 					lines.write[i * 2 + 1] = md.vertices[md.edges[i].vertex_b];
 				}
-				p_gizmo->add_lines(lines, material);
+				p_gizmo->add_lines(lines, material, false, collision_color);
 				p_gizmo->add_collision_segments(lines);
 			}
 		}
@@ -558,7 +599,7 @@ void CollisionShape3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
 	if (Object::cast_to<ConcavePolygonShape3D>(*s)) {
 		Ref<ConcavePolygonShape3D> cs2 = s;
 		Ref<ArrayMesh> mesh = cs2->get_debug_mesh();
-		p_gizmo->add_mesh(mesh, material);
+		p_gizmo->add_lines(cs2->get_debug_mesh_lines(), material, false, collision_color);
 		p_gizmo->add_collision_segments(cs2->get_debug_mesh_lines());
 	}
 
@@ -569,7 +610,7 @@ void CollisionShape3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
 			Vector3(),
 			Vector3(0, 0, rs->get_length())
 		};
-		p_gizmo->add_lines(points, material);
+		p_gizmo->add_lines(points, material, false, collision_color);
 		p_gizmo->add_collision_segments(points);
 		Vector<Vector3> handles;
 		handles.push_back(Vector3(0, 0, rs->get_length()));
@@ -579,7 +620,7 @@ void CollisionShape3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
 	if (Object::cast_to<HeightMapShape3D>(*s)) {
 		Ref<HeightMapShape3D> hms = s;
 
-		Ref<ArrayMesh> mesh = hms->get_debug_mesh();
-		p_gizmo->add_mesh(mesh, material);
+		Vector<Vector3> lines = hms->get_debug_mesh_lines();
+		p_gizmo->add_lines(lines, material, false, collision_color);
 	}
 }

editor/plugins/gizmos/collision_shape_3d_gizmo_plugin.h

@@ -38,6 +38,8 @@ class Gizmo3DHelper;
 class CollisionShape3DGizmoPlugin : public EditorNode3DGizmoPlugin {
 	GDCLASS(CollisionShape3DGizmoPlugin, EditorNode3DGizmoPlugin);
 
+	void create_collision_material(const String &p_name, float p_alpha);
+
 	Ref<Gizmo3DHelper> helper;
 
 public:

editor/plugins/node_3d_editor_gizmos.cpp

@@ -292,14 +292,11 @@ void EditorNode3DGizmo::add_vertices(const Vector<Vector3> &p_vertices, const Re
 
 	Vector<Color> color;
 	color.resize(p_vertices.size());
+	const Color vertex_color = (is_selected() ? Color(1, 1, 1, 0.8) : Color(1, 1, 1, 0.2)) * p_modulate;
 	{
 		Color *w = color.ptrw();
 		for (int i = 0; i < p_vertices.size(); i++) {
-			if (is_selected()) {
-				w[i] = Color(1, 1, 1, 0.8) * p_modulate;
-			} else {
-				w[i] = Color(1, 1, 1, 0.2) * p_modulate;
-			}
+			w[i] = vertex_color;
 		}
 	}
 

scene/3d/physics/collision_shape_3d.cpp

@@ -81,12 +81,19 @@ void CollisionShape3D::_update_in_shape_owner(bool p_xform_only) {
 void CollisionShape3D::_notification(int p_what) {
 	switch (p_what) {
 		case NOTIFICATION_PARENTED: {
+#ifdef DEBUG_ENABLED
+			if (debug_color == get_placeholder_default_color()) {
+				debug_color = SceneTree::get_singleton()->get_debug_collisions_color();
+			}
+#endif // DEBUG_ENABLED
+
 			collision_object = Object::cast_to<CollisionObject3D>(get_parent());
 			if (collision_object) {
 				owner_id = collision_object->create_shape_owner(this);
 				if (shape.is_valid()) {
 					collision_object->shape_owner_add_shape(owner_id, shape);
 				}
+
 				_update_in_shape_owner();
 			}
 		} break;
@@ -166,11 +173,26 @@ void CollisionShape3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("get_shape"), &CollisionShape3D::get_shape);
 	ClassDB::bind_method(D_METHOD("set_disabled", "enable"), &CollisionShape3D::set_disabled);
 	ClassDB::bind_method(D_METHOD("is_disabled"), &CollisionShape3D::is_disabled);
+
 	ClassDB::bind_method(D_METHOD("make_convex_from_siblings"), &CollisionShape3D::make_convex_from_siblings);
 	ClassDB::set_method_flags("CollisionShape3D", "make_convex_from_siblings", METHOD_FLAGS_DEFAULT | METHOD_FLAG_EDITOR);
 
 	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "shape", PROPERTY_HINT_RESOURCE_TYPE, "Shape3D"), "set_shape", "get_shape");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "disabled"), "set_disabled", "is_disabled");
+
+#ifdef DEBUG_ENABLED
+	ClassDB::bind_method(D_METHOD("set_debug_color", "color"), &CollisionShape3D::set_debug_color);
+	ClassDB::bind_method(D_METHOD("get_debug_color"), &CollisionShape3D::get_debug_color);
+
+	ClassDB::bind_method(D_METHOD("set_enable_debug_fill", "enable"), &CollisionShape3D::set_debug_fill_enabled);
+	ClassDB::bind_method(D_METHOD("get_enable_debug_fill"), &CollisionShape3D::get_debug_fill_enabled);
+
+	ADD_PROPERTY(PropertyInfo(Variant::COLOR, "debug_color"), "set_debug_color", "get_debug_color");
+	// Default value depends on a project setting, override for doc generation purposes.
+	ADD_PROPERTY_DEFAULT("debug_color", get_placeholder_default_color());
+
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "debug_fill"), "set_enable_debug_fill", "get_enable_debug_fill");
+#endif // DEBUG_ENABLED
 }
 
 void CollisionShape3D::set_shape(const Ref<Shape3D> &p_shape) {
@@ -178,11 +200,27 @@ void CollisionShape3D::set_shape(const Ref<Shape3D> &p_shape) {
 		return;
 	}
 	if (shape.is_valid()) {
+		shape->disconnect_changed(callable_mp(this, &CollisionShape3D::shape_changed));
 		shape->disconnect_changed(callable_mp((Node3D *)this, &Node3D::update_gizmos));
 	}
 	shape = p_shape;
 	if (shape.is_valid()) {
+#ifdef DEBUG_ENABLED
+		if (shape->get_debug_color() != get_placeholder_default_color()) {
+			set_debug_color(shape->get_debug_color());
+			set_debug_fill_enabled(shape->get_debug_fill());
+		} else if (get_debug_color() != get_placeholder_default_color()) {
+			shape->set_debug_color(debug_color);
+			shape->set_debug_fill(debug_fill);
+		} else {
+			set_debug_color(SceneTree::get_singleton()->get_debug_collisions_color());
+			shape->set_debug_color(SceneTree::get_singleton()->get_debug_collisions_color());
+			shape->set_debug_fill(debug_fill);
+		}
+#endif // DEBUG_ENABLED
+
 		shape->connect_changed(callable_mp((Node3D *)this, &Node3D::update_gizmos));
+		shape->connect_changed(callable_mp(this, &CollisionShape3D::shape_changed));
 	}
 	update_gizmos();
 	if (collision_object) {
@@ -215,6 +253,66 @@ bool CollisionShape3D::is_disabled() const {
 	return disabled;
 }
 
+#ifdef DEBUG_ENABLED
+void CollisionShape3D::set_debug_color(const Color &p_color) {
+	if (p_color == get_placeholder_default_color()) {
+		debug_color = SceneTree::get_singleton()->get_debug_collisions_color();
+	} else if (debug_color != p_color) {
+		debug_color = p_color;
+
+		if (shape.is_valid()) {
+			shape->set_debug_color(p_color);
+		}
+	}
+}
+
+Color CollisionShape3D::get_debug_color() const {
+	return debug_color;
+}
+
+void CollisionShape3D::set_debug_fill_enabled(bool p_enable) {
+	if (debug_fill == p_enable) {
+		return;
+	}
+
+	debug_fill = p_enable;
+
+	if (shape.is_valid()) {
+		shape->set_debug_fill(p_enable);
+	}
+}
+
+bool CollisionShape3D::get_debug_fill_enabled() const {
+	return debug_fill;
+}
+
+bool CollisionShape3D::_property_can_revert(const StringName &p_name) const {
+	if (p_name == "debug_color") {
+		return true;
+	}
+	return false;
+}
+
+bool CollisionShape3D::_property_get_revert(const StringName &p_name, Variant &r_property) const {
+	if (p_name == "debug_color") {
+		r_property = SceneTree::get_singleton()->get_debug_collisions_color();
+		return true;
+	}
+	return false;
+}
+#endif // DEBUG_ENABLED
+
+void CollisionShape3D::shape_changed() {
+#ifdef DEBUG_ENABLED
+	if (shape->get_debug_color() != debug_color) {
+		set_debug_color(shape->get_debug_color());
+	}
+	if (shape->get_debug_fill() != debug_fill) {
+		set_debug_fill_enabled(shape->get_debug_fill());
+	}
+#endif // DEBUG_ENABLED
+}
+
 CollisionShape3D::CollisionShape3D() {
 	//indicator = RenderingServer::get_singleton()->mesh_create();
 	set_notify_local_transform(true);

scene/3d/physics/collision_shape_3d.h

@@ -43,6 +43,13 @@ class CollisionShape3D : public Node3D {
 	uint32_t owner_id = 0;
 	CollisionObject3D *collision_object = nullptr;
 
+#ifdef DEBUG_ENABLED
+	Color debug_color = get_placeholder_default_color();
+	bool debug_fill = true;
+
+	static const Color get_placeholder_default_color() { return Color(0.0, 0.0, 0.0, 0.0); }
+#endif // DEBUG_ENABLED
+
 #ifndef DISABLE_DEPRECATED
 	void resource_changed(Ref<Resource> res);
 #endif
@@ -55,6 +62,13 @@ protected:
 	void _notification(int p_what);
 	static void _bind_methods();
 
+#ifdef DEBUG_ENABLED
+	bool _property_can_revert(const StringName &p_name) const;
+	bool _property_get_revert(const StringName &p_name, Variant &r_property) const;
+#endif // DEBUG_ENABLED
+
+	void shape_changed();
+
 public:
 	void make_convex_from_siblings();
 
@@ -64,6 +78,14 @@ public:
 	void set_disabled(bool p_disabled);
 	bool is_disabled() const;
 
+#ifdef DEBUG_ENABLED
+	void set_debug_color(const Color &p_color);
+	Color get_debug_color() const;
+
+	void set_debug_fill_enabled(bool p_enable);
+	bool get_debug_fill_enabled() const;
+#endif // DEBUG_ENABLED
+
 	PackedStringArray get_configuration_warnings() const override;
 
 	CollisionShape3D();

scene/resources/3d/box_shape_3d.cpp

@@ -29,6 +29,8 @@
 /**************************************************************************/
 
 #include "box_shape_3d.h"
+
+#include "scene/resources/3d/primitive_meshes.h"
 #include "servers/physics_server_3d.h"
 
 Vector<Vector3> BoxShape3D::get_debug_mesh_lines() const {
@@ -47,6 +49,24 @@ Vector<Vector3> BoxShape3D::get_debug_mesh_lines() const {
 	return lines;
 }
 
+Ref<ArrayMesh> BoxShape3D::get_debug_arraymesh_faces(const Color &p_modulate) const {
+	Array box_array;
+	box_array.resize(RS::ARRAY_MAX);
+	BoxMesh::create_mesh_array(box_array, size);
+
+	Vector<Color> colors;
+	const PackedVector3Array &verts = box_array[RS::ARRAY_VERTEX];
+	const int32_t verts_size = verts.size();
+	for (int i = 0; i < verts_size; i++) {
+		colors.append(p_modulate);
+	}
+
+	Ref<ArrayMesh> box_mesh = memnew(ArrayMesh);
+	box_array[RS::ARRAY_COLOR] = colors;
+	box_mesh->add_surface_from_arrays(Mesh::PRIMITIVE_TRIANGLES, box_array);
+	return box_mesh;
+}
+
 real_t BoxShape3D::get_enclosing_radius() const {
 	return size.length() / 2;
 }

scene/resources/3d/box_shape_3d.h

@@ -51,6 +51,7 @@ public:
 	Vector3 get_size() const;
 
 	virtual Vector<Vector3> get_debug_mesh_lines() const override;
+	virtual Ref<ArrayMesh> get_debug_arraymesh_faces(const Color &p_modulate) const override;
 	virtual real_t get_enclosing_radius() const override;
 
 	BoxShape3D();

scene/resources/3d/capsule_shape_3d.cpp

@@ -30,6 +30,7 @@
 
 #include "capsule_shape_3d.h"
 
+#include "scene/resources/3d/primitive_meshes.h"
 #include "servers/physics_server_3d.h"
 
 Vector<Vector3> CapsuleShape3D::get_debug_mesh_lines() const {
@@ -67,6 +68,24 @@ Vector<Vector3> CapsuleShape3D::get_debug_mesh_lines() const {
 	return points;
 }
 
+Ref<ArrayMesh> CapsuleShape3D::get_debug_arraymesh_faces(const Color &p_modulate) const {
+	Array capsule_array;
+	capsule_array.resize(RS::ARRAY_MAX);
+	CapsuleMesh::create_mesh_array(capsule_array, radius, height, 32, 8);
+
+	Vector<Color> colors;
+	const PackedVector3Array &verts = capsule_array[RS::ARRAY_VERTEX];
+	const int32_t verts_size = verts.size();
+	for (int i = 0; i < verts_size; i++) {
+		colors.append(p_modulate);
+	}
+
+	Ref<ArrayMesh> capsule_mesh = memnew(ArrayMesh);
+	capsule_array[RS::ARRAY_COLOR] = colors;
+	capsule_mesh->add_surface_from_arrays(Mesh::PRIMITIVE_TRIANGLES, capsule_array);
+	return capsule_mesh;
+}
+
 real_t CapsuleShape3D::get_enclosing_radius() const {
 	return height * 0.5;
 }

scene/resources/3d/capsule_shape_3d.h

@@ -33,6 +33,8 @@
 
 #include "scene/resources/3d/shape_3d.h"
 
+class ArrayMesh;
+
 class CapsuleShape3D : public Shape3D {
 	GDCLASS(CapsuleShape3D, Shape3D);
 	float radius = 0.5;
@@ -50,6 +52,7 @@ public:
 	float get_height() const;
 
 	virtual Vector<Vector3> get_debug_mesh_lines() const override;
+	virtual Ref<ArrayMesh> get_debug_arraymesh_faces(const Color &p_modulate) const override;
 	virtual real_t get_enclosing_radius() const override;
 
 	CapsuleShape3D();

scene/resources/3d/concave_polygon_shape_3d.cpp

@@ -30,6 +30,7 @@
 
 #include "concave_polygon_shape_3d.h"
 
+#include "scene/resources/mesh.h"
 #include "servers/physics_server_3d.h"
 
 Vector<Vector3> ConcavePolygonShape3D::get_debug_mesh_lines() const {
@@ -59,6 +60,23 @@ Vector<Vector3> ConcavePolygonShape3D::get_debug_mesh_lines() const {
 	return points;
 }
 
+Ref<ArrayMesh> ConcavePolygonShape3D::get_debug_arraymesh_faces(const Color &p_modulate) const {
+	Vector<Color> colors;
+
+	for (int i = 0; i < faces.size(); i++) {
+		colors.push_back(p_modulate);
+	}
+
+	Ref<ArrayMesh> mesh = memnew(ArrayMesh);
+	Array a;
+	a.resize(Mesh::ARRAY_MAX);
+	a[RS::ARRAY_VERTEX] = faces;
+	a[RS::ARRAY_COLOR] = colors;
+	mesh->add_surface_from_arrays(Mesh::PRIMITIVE_TRIANGLES, a);
+
+	return mesh;
+}
+
 real_t ConcavePolygonShape3D::get_enclosing_radius() const {
 	Vector<Vector3> data = get_faces();
 	const Vector3 *read = data.ptr();

scene/resources/3d/concave_polygon_shape_3d.h

@@ -33,6 +33,8 @@
 
 #include "scene/resources/3d/shape_3d.h"
 
+class ArrayMesh;
+
 class ConcavePolygonShape3D : public Shape3D {
 	GDCLASS(ConcavePolygonShape3D, Shape3D);
 
@@ -72,6 +74,7 @@ public:
 	bool is_backface_collision_enabled() const;
 
 	virtual Vector<Vector3> get_debug_mesh_lines() const override;
+	virtual Ref<ArrayMesh> get_debug_arraymesh_faces(const Color &p_modulate) const override;
 	virtual real_t get_enclosing_radius() const override;
 
 	ConcavePolygonShape3D();

scene/resources/3d/convex_polygon_shape_3d.cpp

@@ -30,6 +30,7 @@
 
 #include "convex_polygon_shape_3d.h"
 #include "core/math/convex_hull.h"
+#include "scene/resources/mesh.h"
 #include "servers/physics_server_3d.h"
 
 Vector<Vector3> ConvexPolygonShape3D::get_debug_mesh_lines() const {
@@ -53,6 +54,44 @@ Vector<Vector3> ConvexPolygonShape3D::get_debug_mesh_lines() const {
 	return Vector<Vector3>();
 }
 
+Ref<ArrayMesh> ConvexPolygonShape3D::get_debug_arraymesh_faces(const Color &p_modulate) const {
+	const Vector<Vector3> hull_points = get_points();
+
+	Vector<Vector3> verts;
+	Vector<Color> colors;
+	Vector<int> indices;
+
+	if (hull_points.size() >= 3) {
+		Geometry3D::MeshData md;
+		Error err = ConvexHullComputer::convex_hull(hull_points, md);
+		if (err == OK) {
+			verts = md.vertices;
+			for (int i = 0; i < verts.size(); i++) {
+				colors.push_back(p_modulate);
+			}
+			for (const Geometry3D::MeshData::Face &face : md.faces) {
+				const int first_point = face.indices[0];
+				const int indices_count = face.indices.size();
+				for (int i = 1; i < indices_count - 1; i++) {
+					indices.push_back(first_point);
+					indices.push_back(face.indices[i]);
+					indices.push_back(face.indices[i + 1]);
+				}
+			}
+		}
+	}
+
+	Ref<ArrayMesh> mesh = memnew(ArrayMesh);
+	Array a;
+	a.resize(Mesh::ARRAY_MAX);
+	a[RS::ARRAY_VERTEX] = verts;
+	a[RS::ARRAY_COLOR] = colors;
+	a[RS::ARRAY_INDEX] = indices;
+	mesh->add_surface_from_arrays(Mesh::PRIMITIVE_TRIANGLES, a);
+
+	return mesh;
+}
+
 real_t ConvexPolygonShape3D::get_enclosing_radius() const {
 	Vector<Vector3> data = get_points();
 	const Vector3 *read = data.ptr();

scene/resources/3d/convex_polygon_shape_3d.h

@@ -33,6 +33,8 @@
 
 #include "scene/resources/3d/shape_3d.h"
 
+class ArrayMesh;
+
 class ConvexPolygonShape3D : public Shape3D {
 	GDCLASS(ConvexPolygonShape3D, Shape3D);
 	Vector<Vector3> points;
@@ -47,6 +49,7 @@ public:
 	Vector<Vector3> get_points() const;
 
 	virtual Vector<Vector3> get_debug_mesh_lines() const override;
+	virtual Ref<ArrayMesh> get_debug_arraymesh_faces(const Color &p_modulate) const override;
 	virtual real_t get_enclosing_radius() const override;
 
 	ConvexPolygonShape3D();

scene/resources/3d/cylinder_shape_3d.cpp

@@ -30,6 +30,7 @@
 
 #include "cylinder_shape_3d.h"
 
+#include "scene/resources/3d/primitive_meshes.h"
 #include "servers/physics_server_3d.h"
 
 Vector<Vector3> CylinderShape3D::get_debug_mesh_lines() const {
@@ -60,6 +61,24 @@ Vector<Vector3> CylinderShape3D::get_debug_mesh_lines() const {
 	return points;
 }
 
+Ref<ArrayMesh> CylinderShape3D::get_debug_arraymesh_faces(const Color &p_modulate) const {
+	Array cylinder_array;
+	cylinder_array.resize(RS::ARRAY_MAX);
+	CylinderMesh::create_mesh_array(cylinder_array, radius, radius, height, 32);
+
+	Vector<Color> colors;
+	const PackedVector3Array &verts = cylinder_array[RS::ARRAY_VERTEX];
+	const int32_t verts_size = verts.size();
+	for (int i = 0; i < verts_size; i++) {
+		colors.append(p_modulate);
+	}
+
+	Ref<ArrayMesh> cylinder_mesh = memnew(ArrayMesh);
+	cylinder_array[RS::ARRAY_COLOR] = colors;
+	cylinder_mesh->add_surface_from_arrays(Mesh::PRIMITIVE_TRIANGLES, cylinder_array);
+	return cylinder_mesh;
+}
+
 real_t CylinderShape3D::get_enclosing_radius() const {
 	return Vector2(radius, height * 0.5).length();
 }

scene/resources/3d/cylinder_shape_3d.h

@@ -33,6 +33,8 @@
 
 #include "scene/resources/3d/shape_3d.h"
 
+class ArrayMesh;
+
 class CylinderShape3D : public Shape3D {
 	GDCLASS(CylinderShape3D, Shape3D);
 	float radius = 0.5;
@@ -49,6 +51,7 @@ public:
 	float get_height() const;
 
 	virtual Vector<Vector3> get_debug_mesh_lines() const override;
+	virtual Ref<ArrayMesh> get_debug_arraymesh_faces(const Color &p_modulate) const override;
 	virtual real_t get_enclosing_radius() const override;
 
 	CylinderShape3D();

scene/resources/3d/height_map_shape_3d.cpp

@@ -31,6 +31,7 @@
 #include "height_map_shape_3d.h"
 
 #include "core/io/image.h"
+#include "scene/resources/mesh.h"
 #include "servers/physics_server_3d.h"
 
 Vector<Vector3> HeightMapShape3D::get_debug_mesh_lines() const {
@@ -82,6 +83,60 @@ Vector<Vector3> HeightMapShape3D::get_debug_mesh_lines() const {
 	return points;
 }
 
+Ref<ArrayMesh> HeightMapShape3D::get_debug_arraymesh_faces(const Color &p_modulate) const {
+	Vector<Vector3> verts;
+	Vector<Color> colors;
+	Vector<int> indices;
+
+	// This will be slow for large maps...
+
+	if ((map_width != 0) && (map_depth != 0)) {
+		Vector2 size = Vector2(map_width - 1, map_depth - 1) * -0.5;
+		const real_t *r = map_data.ptr();
+
+		for (int d = 0; d <= map_depth - 2; d++) {
+			const int this_row_offset = map_width * d;
+			const int next_row_offset = this_row_offset + map_width;
+
+			for (int w = 0; w <= map_width - 2; w++) {
+				const float height_tl = r[next_row_offset + w];
+				const float height_bl = r[this_row_offset + w];
+				const float height_br = r[this_row_offset + w + 1];
+				const float height_tr = r[next_row_offset + w + 1];
+
+				const int index_offset = verts.size();
+
+				verts.push_back(Vector3(size.x + w, height_tl, size.y + d + 1));
+				verts.push_back(Vector3(size.x + w, height_bl, size.y + d));
+				verts.push_back(Vector3(size.x + w + 1, height_br, size.y + d));
+				verts.push_back(Vector3(size.x + w + 1, height_tr, size.y + d + 1));
+
+				colors.push_back(p_modulate);
+				colors.push_back(p_modulate);
+				colors.push_back(p_modulate);
+				colors.push_back(p_modulate);
+
+				indices.push_back(index_offset);
+				indices.push_back(index_offset + 1);
+				indices.push_back(index_offset + 2);
+				indices.push_back(index_offset);
+				indices.push_back(index_offset + 2);
+				indices.push_back(index_offset + 3);
+			}
+		}
+	}
+
+	Ref<ArrayMesh> mesh = memnew(ArrayMesh);
+	Array a;
+	a.resize(Mesh::ARRAY_MAX);
+	a[RS::ARRAY_VERTEX] = verts;
+	a[RS::ARRAY_COLOR] = colors;
+	a[RS::ARRAY_INDEX] = indices;
+	mesh->add_surface_from_arrays(Mesh::PRIMITIVE_TRIANGLES, a);
+
+	return mesh;
+}
+
 real_t HeightMapShape3D::get_enclosing_radius() const {
 	return Vector3(real_t(map_width), max_height - min_height, real_t(map_depth)).length();
 }

scene/resources/3d/height_map_shape_3d.h

@@ -33,6 +33,7 @@
 
 #include "scene/resources/3d/shape_3d.h"
 
+class ArrayMesh;
 class Image;
 
 class HeightMapShape3D : public Shape3D {
@@ -62,6 +63,7 @@ public:
 	void update_map_data_from_image(const Ref<Image> &p_image, real_t p_height_min, real_t p_height_max);
 
 	virtual Vector<Vector3> get_debug_mesh_lines() const override;
+	virtual Ref<ArrayMesh> get_debug_arraymesh_faces(const Color &p_modulate) const override;
 	virtual real_t get_enclosing_radius() const override;
 
 	HeightMapShape3D();

scene/resources/3d/separation_ray_shape_3d.cpp

@@ -30,6 +30,7 @@
 
 #include "separation_ray_shape_3d.h"
 
+#include "scene/resources/mesh.h"
 #include "servers/physics_server_3d.h"
 
 Vector<Vector3> SeparationRayShape3D::get_debug_mesh_lines() const {
@@ -41,6 +42,10 @@ Vector<Vector3> SeparationRayShape3D::get_debug_mesh_lines() const {
 	return points;
 }
 
+Ref<ArrayMesh> SeparationRayShape3D::get_debug_arraymesh_faces(const Color &p_modulate) const {
+	return memnew(ArrayMesh);
+}
+
 real_t SeparationRayShape3D::get_enclosing_radius() const {
 	return length;
 }

scene/resources/3d/separation_ray_shape_3d.h

@@ -33,6 +33,8 @@
 
 #include "scene/resources/3d/shape_3d.h"
 
+class ArrayMesh;
+
 class SeparationRayShape3D : public Shape3D {
 	GDCLASS(SeparationRayShape3D, Shape3D);
 	float length = 1.0;
@@ -50,6 +52,7 @@ public:
 	bool get_slide_on_slope() const;
 
 	virtual Vector<Vector3> get_debug_mesh_lines() const override;
+	virtual Ref<ArrayMesh> get_debug_arraymesh_faces(const Color &p_modulate) const override;
 	virtual real_t get_enclosing_radius() const override;
 
 	SeparationRayShape3D();

scene/resources/3d/shape_3d.cpp

@@ -66,6 +66,34 @@ void Shape3D::set_margin(real_t p_margin) {
 	PhysicsServer3D::get_singleton()->shape_set_margin(shape, margin);
 }
 
+#ifdef DEBUG_ENABLED
+void Shape3D::set_debug_color(const Color &p_color) {
+	if (p_color == debug_color) {
+		return;
+	}
+
+	debug_color = p_color;
+	_update_shape();
+}
+
+Color Shape3D::get_debug_color() const {
+	return debug_color;
+}
+
+void Shape3D::set_debug_fill(bool p_fill) {
+	if (p_fill == debug_fill) {
+		return;
+	}
+
+	debug_fill = p_fill;
+	_update_shape();
+}
+
+bool Shape3D::get_debug_fill() const {
+	return debug_fill;
+}
+#endif // DEBUG_ENABLED
+
 Ref<ArrayMesh> Shape3D::get_debug_mesh() {
 	if (debug_mesh_cache.is_valid()) {
 		return debug_mesh_cache;
@@ -79,29 +107,57 @@ Ref<ArrayMesh> Shape3D::get_debug_mesh() {
 		//make mesh
 		Vector<Vector3> array;
 		array.resize(lines.size());
-		{
-			Vector3 *w = array.ptrw();
-			for (int i = 0; i < lines.size(); i++) {
-				w[i] = lines[i];
-			}
+		Vector3 *v = array.ptrw();
+
+		Vector<Color> arraycol;
+		arraycol.resize(lines.size());
+		Color *c = arraycol.ptrw();
+
+		for (int i = 0; i < lines.size(); i++) {
+			v[i] = lines[i];
+			c[i] = debug_color;
 		}
 
-		Array arr;
-		arr.resize(Mesh::ARRAY_MAX);
-		arr[Mesh::ARRAY_VERTEX] = array;
+		Array lines_array;
+		lines_array.resize(Mesh::ARRAY_MAX);
+		lines_array[Mesh::ARRAY_VERTEX] = array;
+		lines_array[Mesh::ARRAY_COLOR] = arraycol;
 
-		SceneTree *st = Object::cast_to<SceneTree>(OS::get_singleton()->get_main_loop());
+		Ref<StandardMaterial3D> material = get_debug_collision_material();
 
-		debug_mesh_cache->add_surface_from_arrays(Mesh::PRIMITIVE_LINES, arr);
+		debug_mesh_cache->add_surface_from_arrays(Mesh::PRIMITIVE_LINES, lines_array);
+		debug_mesh_cache->surface_set_material(0, material);
 
-		if (st) {
-			debug_mesh_cache->surface_set_material(0, st->get_debug_collision_material());
+		if (debug_fill) {
+			Array solid_array = get_debug_arraymesh_faces(debug_color * Color(1.0, 1.0, 1.0, 0.0625))->surface_get_arrays(0);
+			debug_mesh_cache->add_surface_from_arrays(Mesh::PRIMITIVE_TRIANGLES, solid_array);
+			debug_mesh_cache->surface_set_material(1, material);
 		}
 	}
 
 	return debug_mesh_cache;
 }
 
+Ref<Material> Shape3D::get_debug_collision_material() {
+	if (collision_material.is_valid()) {
+		return collision_material;
+	}
+
+	Ref<StandardMaterial3D> material = memnew(StandardMaterial3D);
+	material->set_albedo(Color(1.0, 1.0, 1.0));
+	material->set_shading_mode(StandardMaterial3D::SHADING_MODE_UNSHADED);
+	material->set_transparency(StandardMaterial3D::TRANSPARENCY_ALPHA);
+	material->set_render_priority(StandardMaterial3D::RENDER_PRIORITY_MIN + 1);
+	material->set_cull_mode(StandardMaterial3D::CULL_BACK);
+	material->set_flag(StandardMaterial3D::FLAG_DISABLE_FOG, true);
+	material->set_flag(StandardMaterial3D::FLAG_ALBEDO_FROM_VERTEX_COLOR, true);
+	material->set_flag(StandardMaterial3D::FLAG_SRGB_VERTEX_COLOR, true);
+
+	collision_material = material;
+
+	return collision_material;
+}
+
 void Shape3D::_update_shape() {
 	emit_changed();
 	debug_mesh_cache.unref();

scene/resources/3d/shape_3d.h

@@ -34,6 +34,7 @@
 #include "core/io/resource.h"
 
 class ArrayMesh;
+class Material;
 
 class Shape3D : public Resource {
 	GDCLASS(Shape3D, Resource);
@@ -44,6 +45,10 @@ class Shape3D : public Resource {
 	real_t margin = 0.04;
 
 	Ref<ArrayMesh> debug_mesh_cache;
+	Ref<Material> collision_material;
+
+	Color debug_color = Color(0.0, 0.0, 0.0, 0.0);
+	bool debug_fill = true;
 
 protected:
 	static void _bind_methods();
@@ -51,6 +56,8 @@ protected:
 	_FORCE_INLINE_ RID get_shape() const { return shape; }
 	Shape3D(RID p_shape);
 
+	Ref<Material> get_debug_collision_material();
+
 	virtual void _update_shape();
 
 public:
@@ -58,6 +65,7 @@ public:
 
 	Ref<ArrayMesh> get_debug_mesh();
 	virtual Vector<Vector3> get_debug_mesh_lines() const = 0; // { return Vector<Vector3>(); }
+	virtual Ref<ArrayMesh> get_debug_arraymesh_faces(const Color &p_modulate) const = 0;
 	/// Returns the radius of a sphere that fully enclose this shape
 	virtual real_t get_enclosing_radius() const = 0;
 
@@ -69,6 +77,14 @@ public:
 	real_t get_margin() const;
 	void set_margin(real_t p_margin);
 
+#ifdef DEBUG_ENABLED
+	void set_debug_color(const Color &p_color);
+	Color get_debug_color() const;
+
+	void set_debug_fill(bool p_fill);
+	bool get_debug_fill() const;
+#endif // DEBUG_ENABLED
+
 	Shape3D();
 	~Shape3D();
 };

scene/resources/3d/sphere_shape_3d.cpp

@@ -30,6 +30,8 @@
 
 #include "sphere_shape_3d.h"
 
+#include "scene/resources/3d/primitive_meshes.h"
+#include "scene/resources/material.h"
 #include "servers/physics_server_3d.h"
 
 Vector<Vector3> SphereShape3D::get_debug_mesh_lines() const {
@@ -54,6 +56,24 @@ Vector<Vector3> SphereShape3D::get_debug_mesh_lines() const {
 	return points;
 }
 
+Ref<ArrayMesh> SphereShape3D::get_debug_arraymesh_faces(const Color &p_modulate) const {
+	Array sphere_array;
+	sphere_array.resize(RS::ARRAY_MAX);
+	SphereMesh::create_mesh_array(sphere_array, radius, radius * 2, 32);
+
+	Vector<Color> colors;
+	const PackedVector3Array &verts = sphere_array[RS::ARRAY_VERTEX];
+	const int32_t verts_size = verts.size();
+	for (int i = 0; i < verts_size; i++) {
+		colors.append(p_modulate);
+	}
+
+	Ref<ArrayMesh> sphere_mesh = memnew(ArrayMesh);
+	sphere_array[RS::ARRAY_COLOR] = colors;
+	sphere_mesh->add_surface_from_arrays(Mesh::PRIMITIVE_TRIANGLES, sphere_array);
+	return sphere_mesh;
+}
+
 real_t SphereShape3D::get_enclosing_radius() const {
 	return radius;
 }

scene/resources/3d/sphere_shape_3d.h

@@ -33,6 +33,8 @@
 
 #include "scene/resources/3d/shape_3d.h"
 
+class ArrayMesh;
+
 class SphereShape3D : public Shape3D {
 	GDCLASS(SphereShape3D, Shape3D);
 	float radius = 0.5f;
@@ -47,6 +49,7 @@ public:
 	float get_radius() const;
 
 	virtual Vector<Vector3> get_debug_mesh_lines() const override;
+	virtual Ref<ArrayMesh> get_debug_arraymesh_faces(const Color &p_modulate) const override;
 	virtual real_t get_enclosing_radius() const override;
 
 	SphereShape3D();

scene/resources/3d/world_boundary_shape_3d.cpp

@@ -30,6 +30,7 @@
 
 #include "world_boundary_shape_3d.h"
 
+#include "scene/resources/mesh.h"
 #include "servers/physics_server_3d.h"
 
 Vector<Vector3> WorldBoundaryShape3D::get_debug_mesh_lines() const {
@@ -61,6 +62,53 @@ Vector<Vector3> WorldBoundaryShape3D::get_debug_mesh_lines() const {
 	return points;
 }
 
+Ref<ArrayMesh> WorldBoundaryShape3D::get_debug_arraymesh_faces(const Color &p_modulate) const {
+	Plane p = get_plane();
+
+	Vector3 n1 = p.get_any_perpendicular_normal();
+	Vector3 n2 = p.normal.cross(n1).normalized();
+
+	Vector3 pface[4] = {
+		p.normal * p.d + n1 * 10.0 + n2 * 10.0,
+		p.normal * p.d + n1 * 10.0 + n2 * -10.0,
+		p.normal * p.d + n1 * -10.0 + n2 * -10.0,
+		p.normal * p.d + n1 * -10.0 + n2 * 10.0,
+	};
+
+	Vector<Vector3> points = {
+		pface[0],
+		pface[1],
+		pface[2],
+		pface[3],
+	};
+
+	Vector<Color> colors = {
+		p_modulate,
+		p_modulate,
+		p_modulate,
+		p_modulate,
+	};
+
+	Vector<int> indices = {
+		0,
+		1,
+		2,
+		0,
+		2,
+		3,
+	};
+
+	Ref<ArrayMesh> mesh = memnew(ArrayMesh);
+	Array a;
+	a.resize(Mesh::ARRAY_MAX);
+	a[RS::ARRAY_VERTEX] = points;
+	a[RS::ARRAY_COLOR] = colors;
+	a[RS::ARRAY_INDEX] = indices;
+	mesh->add_surface_from_arrays(Mesh::PRIMITIVE_TRIANGLES, a);
+
+	return mesh;
+}
+
 void WorldBoundaryShape3D::_update_shape() {
 	PhysicsServer3D::get_singleton()->shape_set_data(get_shape(), plane);
 	Shape3D::_update_shape();

scene/resources/3d/world_boundary_shape_3d.h

@@ -33,6 +33,8 @@
 
 #include "scene/resources/3d/shape_3d.h"
 
+class ArrayMesh;
+
 class WorldBoundaryShape3D : public Shape3D {
 	GDCLASS(WorldBoundaryShape3D, Shape3D);
 	Plane plane;
@@ -46,6 +48,7 @@ public:
 	const Plane &get_plane() const;
 
 	virtual Vector<Vector3> get_debug_mesh_lines() const override;
+	virtual Ref<ArrayMesh> get_debug_arraymesh_faces(const Color &p_modulate) const override;
 	virtual real_t get_enclosing_radius() const override {
 		// Should be infinite?
 		return 0;