Ability to visually debug geometry visually:

-Visible 2D and 3D Shapes, Polygons, Tile collisions, etc.
-Visible Navmesh and Navpoly
-Visible collision contacts for 2D and 3D as a red point
-Customizable colors in project settings

4 files changed, 298 insertions, 3 deletions

diff --git a/scene/main/scene_main_loop.cpp b/scene/main/scene_main_loop.cpp
index ead729c2bf..adf053f5c9 100644
--- a/scene/main/scene_main_loop.cpp
+++ b/scene/main/scene_main_loop.cpp
@@ -42,6 +42,8 @@
 #include "io/resource_loader.h"
 #include "viewport.h"
 #include "scene/resources/packed_scene.h"
+#include "scene/resources/material.h"
+#include "scene/resources/mesh.h"
 
 void SceneTree::tree_changed() {
 
@@ -470,8 +472,6 @@ void SceneTree::init() {
 	input_handled=false;
 
 
-	editor_hint=false;
-	debug_collisions_hint=false;
 	pause=false;
 
 	root->_set_tree(this);
@@ -635,6 +635,165 @@ bool SceneTree::is_debugging_collisions_hint() const {
 	return debug_collisions_hint;
 }
 
+void SceneTree::set_debug_navigation_hint(bool p_enabled) {
+
+	debug_navigation_hint=p_enabled;
+}
+
+bool SceneTree::is_debugging_navigation_hint() const {
+
+	return debug_navigation_hint;
+}
+
+void SceneTree::set_debug_collisions_color(const Color& p_color) {
+
+	debug_collisions_color=p_color;
+}
+
+Color SceneTree::get_debug_collisions_color() const {
+
+	return debug_collisions_color;
+}
+
+void SceneTree::set_debug_collision_contact_color(const Color& p_color) {
+
+	debug_collision_contact_color=p_color;
+}
+
+Color SceneTree::get_debug_collision_contact_color() const {
+
+	return debug_collision_contact_color;
+}
+
+void SceneTree::set_debug_navigation_color(const Color& p_color) {
+
+	debug_navigation_color=p_color;
+}
+
+Color SceneTree::get_debug_navigation_color() const {
+
+	return debug_navigation_color;
+}
+
+void SceneTree::set_debug_navigation_disabled_color(const Color& p_color) {
+
+	debug_navigation_disabled_color=p_color;
+}
+
+Color SceneTree::get_debug_navigation_disabled_color() const {
+
+	return debug_navigation_disabled_color;
+}
+
+Ref<Material> SceneTree::get_debug_navigation_material() {
+
+	if (navigation_material.is_valid())
+		return navigation_material;
+
+	Ref<FixedMaterial> line_material = Ref<FixedMaterial>( memnew( FixedMaterial ));
+	line_material->set_flag(Material::FLAG_UNSHADED, true);
+	line_material->set_line_width(3.0);
+	line_material->set_fixed_flag(FixedMaterial::FLAG_USE_ALPHA, true);
+	line_material->set_fixed_flag(FixedMaterial::FLAG_USE_COLOR_ARRAY, true);
+	line_material->set_parameter(FixedMaterial::PARAM_DIFFUSE,get_debug_navigation_color());
+
+	navigation_material=line_material;
+
+	return navigation_material;
+
+}
+
+Ref<Material> SceneTree::get_debug_navigation_disabled_material(){
+
+	if (navigation_disabled_material.is_valid())
+		return navigation_disabled_material;
+
+	Ref<FixedMaterial> line_material = Ref<FixedMaterial>( memnew( FixedMaterial ));
+	line_material->set_flag(Material::FLAG_UNSHADED, true);
+	line_material->set_line_width(3.0);
+	line_material->set_fixed_flag(FixedMaterial::FLAG_USE_ALPHA, true);
+	line_material->set_fixed_flag(FixedMaterial::FLAG_USE_COLOR_ARRAY, true);
+	line_material->set_parameter(FixedMaterial::PARAM_DIFFUSE,get_debug_navigation_disabled_color());
+
+	navigation_disabled_material=line_material;
+
+	return navigation_disabled_material;
+
+}
+Ref<Material> SceneTree::get_debug_collision_material() {
+
+	if (collision_material.is_valid())
+		return collision_material;
+
+
+	Ref<FixedMaterial> line_material = Ref<FixedMaterial>( memnew( FixedMaterial ));
+	line_material->set_flag(Material::FLAG_UNSHADED, true);
+	line_material->set_line_width(3.0);
+	line_material->set_fixed_flag(FixedMaterial::FLAG_USE_ALPHA, true);
+	line_material->set_fixed_flag(FixedMaterial::FLAG_USE_COLOR_ARRAY, true);
+	line_material->set_parameter(FixedMaterial::PARAM_DIFFUSE,get_debug_collisions_color());
+
+	collision_material=line_material;
+
+	return collision_material;
+}
+
+Ref<Mesh> SceneTree::get_debug_contact_mesh() {
+
+	if (debug_contact_mesh.is_valid())
+		return debug_contact_mesh;
+
+	debug_contact_mesh = Ref<Mesh>( memnew( Mesh ) );
+
+	Ref<FixedMaterial> mat = memnew( FixedMaterial );
+	mat->set_flag(Material::FLAG_UNSHADED,true);
+	mat->set_flag(Material::FLAG_DOUBLE_SIDED,true);
+	mat->set_fixed_flag(FixedMaterial::FLAG_USE_ALPHA,true);
+	mat->set_parameter(FixedMaterial::PARAM_DIFFUSE,get_debug_collision_contact_color());
+
+	Vector3 diamond[6]={
+		Vector3(-1, 0, 0),
+		Vector3( 1, 0, 0),
+		Vector3( 0, -1, 0),
+		Vector3( 0, 1, 0),
+		Vector3( 0, 0, -1),
+		Vector3( 0, 0, 1)
+	};
+
+	int diamond_faces[8*3]={
+		0,2,4,
+		0,3,4,
+		1,2,4,
+		1,3,4,
+		0,2,5,
+		0,3,5,
+		1,2,5,
+		1,3,5,
+	};
+
+	DVector<int> indices;
+	for(int i=0;i<8*3;i++)
+		indices.push_back(diamond_faces[i]);
+
+	DVector<Vector3> vertices;
+	for(int i=0;i<6;i++)
+		vertices.push_back(diamond[i]*0.1);
+
+	Array arr;
+	arr.resize(Mesh::ARRAY_MAX);
+	arr[Mesh::ARRAY_VERTEX]=vertices;
+	arr[Mesh::ARRAY_INDEX]=indices;
+
+
+	debug_contact_mesh->add_surface(Mesh::PRIMITIVE_TRIANGLES,arr);
+	debug_contact_mesh->surface_set_material(0,mat);
+
+	return debug_contact_mesh;
+
+}
+
+
+
 void SceneTree::set_pause(bool p_enabled) {
 
 	if (p_enabled==pause)
@@ -1437,6 +1596,9 @@ void SceneTree::_bind_methods() {
 	ObjectTypeDB::bind_method(_MD("is_editor_hint"),&SceneTree::is_editor_hint);
 	ObjectTypeDB::bind_method(_MD("set_debug_collisions_hint","enable"),&SceneTree::set_debug_collisions_hint);
 	ObjectTypeDB::bind_method(_MD("is_debugging_collisions_hint"),&SceneTree::is_debugging_collisions_hint);
+	ObjectTypeDB::bind_method(_MD("set_debug_navigation_hint","enable"),&SceneTree::set_debug_navigation_hint);
+	ObjectTypeDB::bind_method(_MD("is_debugging_navigation_hint"),&SceneTree::is_debugging_navigation_hint);
+
 #ifdef TOOLS_ENABLED
 	ObjectTypeDB::bind_method(_MD("set_edited_scene_root","scene"),&SceneTree::set_edited_scene_root);
 	ObjectTypeDB::bind_method(_MD("get_edited_scene_root"),&SceneTree::get_edited_scene_root);
@@ -1503,10 +1665,23 @@ void SceneTree::_bind_methods() {
 
 }
 
+SceneTree *SceneTree::singleton=NULL;
+
 SceneTree::SceneTree() {
 
+	singleton=this;
 	_quit=false;
 	initialized=false;
+	editor_hint=false;
+	debug_collisions_hint=false;
+	debug_navigation_hint=false;
+	debug_collisions_color=GLOBAL_DEF("debug/collision_shape_color",Color(0.0,0.6,0.7,0.5));
+	debug_collision_contact_color=GLOBAL_DEF("debug/collision_contact_color",Color(1.0,0.2,0.1,0.8));
+	debug_navigation_color=GLOBAL_DEF("debug/navigation_geometry_color",Color(0.1,1.0,0.7,0.4));
+	debug_navigation_disabled_color=GLOBAL_DEF("debug/navigation_disabled_geometry_color",Color(1.0,0.7,0.1,0.4));
+	collision_debug_contacts=GLOBAL_DEF("debug/collision_max_contacts_displayed",10000);
+
+
 	tree_version=1;
 	fixed_process_time=1;
 	idle_process_time=1;
diff --git a/scene/main/scene_main_loop.h b/scene/main/scene_main_loop.h
index b44456bf1d..8d9021d24e 100644
--- a/scene/main/scene_main_loop.h
+++ b/scene/main/scene_main_loop.h
@@ -45,6 +45,8 @@ class SceneTree;
 class PackedScene;
 class Node;
 class Viewport;
+class Material;
+class Mesh;
 
 class SceneTree : public MainLoop {
 
@@ -88,6 +90,7 @@ private:
 
 	bool editor_hint;
 	bool debug_collisions_hint;
+	bool debug_navigation_hint;
 	bool pause;
 	int root_lock;
 
@@ -139,9 +142,20 @@ private:
 
 	Node *current_scene;
 
+	Color debug_collisions_color;
+	Color debug_collision_contact_color;
+	Color debug_navigation_color;
+	Color debug_navigation_disabled_color;
+	Ref<Mesh> debug_contact_mesh;
+	Ref<Material> navigation_material;
+	Ref<Material> navigation_disabled_material;
+	Ref<Material> collision_material;
+	int collision_debug_contacts;
+
 	void _change_scene(Node* p_to);
 	//void _call_group(uint32_t p_call_flags,const StringName& p_group,const StringName& p_function,const Variant& p_arg1,const Variant& p_arg2);
 
+	static SceneTree *singleton;
 friend class Node;
 
 	void tree_changed();
@@ -274,6 +288,29 @@ public:
 	void set_debug_collisions_hint(bool p_enabled);
 	bool is_debugging_collisions_hint() const;
 
+	void set_debug_navigation_hint(bool p_enabled);
+	bool is_debugging_navigation_hint() const;
+
+	void set_debug_collisions_color(const Color& p_color);
+	Color get_debug_collisions_color() const;
+
+	void set_debug_collision_contact_color(const Color& p_color);
+	Color get_debug_collision_contact_color() const;
+
+	void set_debug_navigation_color(const Color& p_color);
+	Color get_debug_navigation_color() const;
+
+	void set_debug_navigation_disabled_color(const Color& p_color);
+	Color get_debug_navigation_disabled_color() const;
+
+
+	Ref<Material> get_debug_navigation_material();
+	Ref<Material> get_debug_navigation_disabled_material();
+	Ref<Material> get_debug_collision_material();
+	Ref<Mesh> get_debug_contact_mesh();
+
+	int get_collision_debug_contact_count() { return collision_debug_contacts; }
+
 	int64_t get_frame() const;
 
 	int get_node_count() const;
@@ -301,6 +338,7 @@ public:
 	//used by Main::start, don't use otherwise
 	void add_current_scene(Node * p_current);
 
+	static SceneTree* get_singleton() { return singleton; }
 
 
 	SceneTree();
diff --git a/scene/main/viewport.cpp b/scene/main/viewport.cpp
index 3bb64e54c6..089f3b5d63 100644
--- a/scene/main/viewport.cpp
+++ b/scene/main/viewport.cpp
@@ -37,6 +37,7 @@
 #include "servers/spatial_sound_2d_server.h"
 #include "scene/gui/control.h"
 #include "scene/3d/camera.h"
+#include "scene/resources/mesh.h"
 #include "scene/3d/spatial_indexer.h"
 #include "scene/3d/collision_object.h"
 
@@ -318,7 +319,23 @@ void Viewport::_notification(int p_what) {
 //				update_worlds();
 			}
 
-			add_to_group("_viewports");
+			if (get_tree()->is_debugging_collisions_hint()) {
+				//2D
+				Physics2DServer::get_singleton()->space_set_debug_contacts(find_world_2d()->get_space(),get_tree()->get_collision_debug_contact_count());
+				contact_2d_debug=VisualServer::get_singleton()->canvas_item_create();
+				VisualServer::get_singleton()->canvas_item_set_parent(contact_2d_debug,find_world_2d()->get_canvas());
+				//3D
+				PhysicsServer::get_singleton()->space_set_debug_contacts(find_world()->get_space(),get_tree()->get_collision_debug_contact_count());
+				contact_3d_debug_multimesh=VisualServer::get_singleton()->multimesh_create();
+				VisualServer::get_singleton()->multimesh_set_instance_count(contact_3d_debug_multimesh,get_tree()->get_collision_debug_contact_count());
+				VisualServer::get_singleton()->multimesh_set_visible_instances(contact_3d_debug_multimesh,0);
+				VisualServer::get_singleton()->multimesh_set_mesh(contact_3d_debug_multimesh,get_tree()->get_debug_contact_mesh()->get_rid());
+				contact_3d_debug_instance=VisualServer::get_singleton()->instance_create();
+				VisualServer::get_singleton()->instance_set_base(contact_3d_debug_instance,contact_3d_debug_multimesh);
+				VisualServer::get_singleton()->instance_set_scenario(contact_3d_debug_instance,find_world()->get_scenario());
+				VisualServer::get_singleton()->instance_geometry_set_flag(contact_3d_debug_instance,VS::INSTANCE_FLAG_VISIBLE_IN_ALL_ROOMS,true);
+
+			}
 
 		} break;
 		case NOTIFICATION_READY: {
@@ -351,11 +368,69 @@ void Viewport::_notification(int p_what) {
 			VisualServer::get_singleton()->viewport_set_scenario(viewport,RID());
 			SpatialSoundServer::get_singleton()->listener_set_space(listener,RID());
 			VisualServer::get_singleton()->viewport_remove_canvas(viewport,current_canvas);
+			if (contact_2d_debug.is_valid()) {
+				VisualServer::get_singleton()->free(contact_2d_debug);
+				contact_2d_debug=RID();
+			}
+
+			if (contact_3d_debug_multimesh.is_valid()) {
+				VisualServer::get_singleton()->free(contact_3d_debug_multimesh);
+				VisualServer::get_singleton()->free(contact_3d_debug_instance);
+				contact_3d_debug_instance=RID();
+				contact_3d_debug_multimesh=RID();
+			}
+
 			remove_from_group("_viewports");
 
 		} break;
 		case NOTIFICATION_FIXED_PROCESS: {
 
+
+			if (get_tree()->is_debugging_collisions_hint() && contact_2d_debug.is_valid()) {
+
+				VisualServer::get_singleton()->canvas_item_clear(contact_2d_debug);
+				VisualServer::get_singleton()->canvas_item_raise(contact_2d_debug);
+
+				Vector<Vector2> points = Physics2DServer::get_singleton()->space_get_contacts(find_world_2d()->get_space());
+				int point_count = Physics2DServer::get_singleton()->space_get_contact_count(find_world_2d()->get_space());
+				Color ccol = get_tree()->get_debug_collision_contact_color();
+
+
+				for(int i=0;i<point_count;i++) {
+
+					VisualServer::get_singleton()->canvas_item_add_rect(contact_2d_debug,Rect2(points[i]-Vector2(2,2),Vector2(5,5)),ccol);
+				}
+			}
+
+			if (get_tree()->is_debugging_collisions_hint() && contact_3d_debug_multimesh.is_valid()) {
+
+
+				Vector<Vector3> points = PhysicsServer::get_singleton()->space_get_contacts(find_world()->get_space());
+				int point_count = PhysicsServer::get_singleton()->space_get_contact_count(find_world()->get_space());
+
+
+				VS::get_singleton()->multimesh_set_visible_instances(contact_3d_debug_multimesh,point_count);
+
+				if (point_count>0) {
+					AABB aabb;
+
+					Transform t;
+					for(int i=0;i<point_count;i++) {
+
+						if (i==0)
+							aabb.pos=points[i];
+						else
+							aabb.expand_to(points[i]);
+						t.origin=points[i];
+						VisualServer::get_singleton()->multimesh_instance_set_transform(contact_3d_debug_multimesh,i,t);
+					}
+					aabb.grow(aabb.get_longest_axis_size()*0.01);
+					VisualServer::get_singleton()->multimesh_set_aabb(contact_3d_debug_multimesh,aabb);
+				}
+			}
+
+
+
 			if (physics_object_picking) {
 
 				Vector2 last_pos(1e20,1e20);
@@ -1449,6 +1524,8 @@ Viewport::Viewport() {
 	unhandled_key_input_group = "_vp_unhandled_key_input"+id;
 
 
+
+
 }
 
 
diff --git a/scene/main/viewport.h b/scene/main/viewport.h
index c3c339ac5d..843a1fd9b7 100644
--- a/scene/main/viewport.h
+++ b/scene/main/viewport.h
@@ -104,6 +104,11 @@ friend class RenderTargetTexture;
 	Rect2 rect;
 	Rect2 to_screen_rect;
 
+	RID contact_2d_debug;
+	RID contact_3d_debug_multimesh;
+	RID contact_3d_debug_instance;
+
+
 
 	bool size_override;
 	bool size_override_stretch;