Add CollisionShape3D custom debug colours

This allows changing the display colour of a CollisionShape3D node on a per-shape basis.
It also adds the ability to display a solid coloured preview of a CollisionShape3D.

Closes https://github.com/godotengine/godot-proposals/issues/906

20 files changed, 351 insertions, 12 deletions

diff --git a/scene/resources/3d/box_shape_3d.cpp b/scene/resources/3d/box_shape_3d.cpp
index 313aeb1bca..afb03a8ba1 100644
--- a/scene/resources/3d/box_shape_3d.cpp
+++ b/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;
 }
diff --git a/scene/resources/3d/box_shape_3d.h b/scene/resources/3d/box_shape_3d.h
index 45c1cde5d7..a9137fdcaf 100644
--- a/scene/resources/3d/box_shape_3d.h
+++ b/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();
diff --git a/scene/resources/3d/capsule_shape_3d.cpp b/scene/resources/3d/capsule_shape_3d.cpp
index 9e16801060..b63bf69aee 100644
--- a/scene/resources/3d/capsule_shape_3d.cpp
+++ b/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;
 }
diff --git a/scene/resources/3d/capsule_shape_3d.h b/scene/resources/3d/capsule_shape_3d.h
index 90ee3b584a..2ad7fa452c 100644
--- a/scene/resources/3d/capsule_shape_3d.h
+++ b/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();
diff --git a/scene/resources/3d/concave_polygon_shape_3d.cpp b/scene/resources/3d/concave_polygon_shape_3d.cpp
index 82b125905f..1254cc4306 100644
--- a/scene/resources/3d/concave_polygon_shape_3d.cpp
+++ b/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();
diff --git a/scene/resources/3d/concave_polygon_shape_3d.h b/scene/resources/3d/concave_polygon_shape_3d.h
index a5e46474d5..d5e5bc394b 100644
--- a/scene/resources/3d/concave_polygon_shape_3d.h
+++ b/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();
diff --git a/scene/resources/3d/convex_polygon_shape_3d.cpp b/scene/resources/3d/convex_polygon_shape_3d.cpp
index 586d5f4678..809c089e6c 100644
--- a/scene/resources/3d/convex_polygon_shape_3d.cpp
+++ b/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();
diff --git a/scene/resources/3d/convex_polygon_shape_3d.h b/scene/resources/3d/convex_polygon_shape_3d.h
index 7d1ac123c6..2dd4ce66db 100644
--- a/scene/resources/3d/convex_polygon_shape_3d.h
+++ b/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();
diff --git a/scene/resources/3d/cylinder_shape_3d.cpp b/scene/resources/3d/cylinder_shape_3d.cpp
index a91282fd33..700ff5884d 100644
--- a/scene/resources/3d/cylinder_shape_3d.cpp
+++ b/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();
 }
diff --git a/scene/resources/3d/cylinder_shape_3d.h b/scene/resources/3d/cylinder_shape_3d.h
index bd57bc2a97..9388cab368 100644
--- a/scene/resources/3d/cylinder_shape_3d.h
+++ b/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();
diff --git a/scene/resources/3d/height_map_shape_3d.cpp b/scene/resources/3d/height_map_shape_3d.cpp
index 5b55b66152..65b1425670 100644
--- a/scene/resources/3d/height_map_shape_3d.cpp
+++ b/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();
 }
diff --git a/scene/resources/3d/height_map_shape_3d.h b/scene/resources/3d/height_map_shape_3d.h
index 33ba9c4472..b5be53092d 100644
--- a/scene/resources/3d/height_map_shape_3d.h
+++ b/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();
diff --git a/scene/resources/3d/separation_ray_shape_3d.cpp b/scene/resources/3d/separation_ray_shape_3d.cpp
index 07e93b8b79..55529be624 100644
--- a/scene/resources/3d/separation_ray_shape_3d.cpp
+++ b/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;
 }
diff --git a/scene/resources/3d/separation_ray_shape_3d.h b/scene/resources/3d/separation_ray_shape_3d.h
index f24f0eae9e..c1c273c448 100644
--- a/scene/resources/3d/separation_ray_shape_3d.h
+++ b/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();
diff --git a/scene/resources/3d/shape_3d.cpp b/scene/resources/3d/shape_3d.cpp
index 259d82b7a0..a1ee7b85dd 100644
--- a/scene/resources/3d/shape_3d.cpp
+++ b/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();
diff --git a/scene/resources/3d/shape_3d.h b/scene/resources/3d/shape_3d.h
index 5e6cdbe421..e956f4c322 100644
--- a/scene/resources/3d/shape_3d.h
+++ b/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();
 };
diff --git a/scene/resources/3d/sphere_shape_3d.cpp b/scene/resources/3d/sphere_shape_3d.cpp
index 56b78471ec..bdce41c16f 100644
--- a/scene/resources/3d/sphere_shape_3d.cpp
+++ b/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;
 }
diff --git a/scene/resources/3d/sphere_shape_3d.h b/scene/resources/3d/sphere_shape_3d.h
index 8e95cea608..cb0685287d 100644
--- a/scene/resources/3d/sphere_shape_3d.h
+++ b/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();
diff --git a/scene/resources/3d/world_boundary_shape_3d.cpp b/scene/resources/3d/world_boundary_shape_3d.cpp
index beaaddc95e..25f060aa97 100644
--- a/scene/resources/3d/world_boundary_shape_3d.cpp
+++ b/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();
diff --git a/scene/resources/3d/world_boundary_shape_3d.h b/scene/resources/3d/world_boundary_shape_3d.h
index 06cff6aa9a..456316df2e 100644
--- a/scene/resources/3d/world_boundary_shape_3d.h
+++ b/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;