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

2 files changed, 70 insertions, 3 deletions

diff --git a/modules/gridmap/grid_map.cpp b/modules/gridmap/grid_map.cpp
index 4f0daf329f..ad7c392cd0 100644
--- a/modules/gridmap/grid_map.cpp
+++ b/modules/gridmap/grid_map.cpp
@@ -34,7 +34,7 @@
 #include "scene/3d/baked_light_instance.h"
 #include "io/marshalls.h"
 #include "scene/scene_string_names.h"
-
+#include "os/os.h"
 
 bool GridMap::_set(const StringName& p_name, const Variant& p_value) {
 
@@ -393,8 +393,12 @@ void GridMap::set_cell_item(int p_x,int p_y,int p_z, int p_item,int p_rot){
 		if (g.items.empty()) {
 
 			PhysicsServer::get_singleton()->free(g.static_body);
+			if (g.collision_debug.is_valid()) {
+				PhysicsServer::get_singleton()->free(g.collision_debug);
+				PhysicsServer::get_singleton()->free(g.collision_debug_instance);
+			}
 
-			memdelete(&g);
+			memdelete(&g);			
 			octant_map.erase(octantkey);
 		} else {
 
@@ -422,6 +426,20 @@ void GridMap::set_cell_item(int p_x,int p_y,int p_z, int p_item,int p_rot){
 		if (is_inside_world())
 			PhysicsServer::get_singleton()->body_set_space(g->static_body,get_world()->get_space());
 
+		SceneTree *st=SceneTree::get_singleton();
+
+		if (st && st->is_debugging_collisions_hint()) {
+
+			g->collision_debug=VisualServer::get_singleton()->mesh_create();
+			g->collision_debug_instance=VisualServer::get_singleton()->instance_create();
+			VisualServer::get_singleton()->instance_set_base(g->collision_debug_instance,g->collision_debug);
+			if (is_inside_world()) {
+				VisualServer::get_singleton()->instance_set_scenario(g->collision_debug_instance,get_world()->get_scenario());
+				VisualServer::get_singleton()->instance_set_transform(g->collision_debug_instance,get_global_transform());
+			}
+
+		}
+
 		octant_map[octantkey]=g;
 	}
 
@@ -512,6 +530,13 @@ void GridMap::_octant_enter_world(const OctantKey &p_key) {
 	//print_line("BODYPOS: "+get_global_transform());
 
 
+	if (g.collision_debug_instance.is_valid()) {
+		VS::get_singleton()->instance_set_scenario(g.collision_debug_instance,get_world()->get_scenario());
+		VS::get_singleton()->instance_set_transform(g.collision_debug_instance,get_global_transform());
+		if (area_map.has(p_key.area)) {
+			VS::get_singleton()->instance_set_room(g.collision_debug_instance,area_map[p_key.area]->instance);
+		}
+	}
 	if (g.baked.is_valid()) {
 
 		Transform xf = get_global_transform();
@@ -545,6 +570,10 @@ void GridMap::_octant_transform(const OctantKey &p_key) {
 	Octant&g = *octant_map[p_key];
 	PhysicsServer::get_singleton()->body_set_state(g.static_body,PhysicsServer::BODY_STATE_TRANSFORM,get_global_transform());
 
+	if (g.collision_debug_instance.is_valid()) {
+		VS::get_singleton()->instance_set_transform(g.collision_debug_instance,get_global_transform());
+	}
+
 	if (g.baked.is_valid()) {
 
 		Transform xf = get_global_transform();
@@ -572,6 +601,13 @@ void GridMap::_octant_update(const OctantKey &p_key) {
 
 	PhysicsServer::get_singleton()->body_clear_shapes(g.static_body);
 
+	if (g.collision_debug.is_valid()) {
+
+		VS::get_singleton()->mesh_clear(g.collision_debug);
+	}
+
+	DVector<Vector3> col_debug;
+
 	for(Map<int,Octant::ItemInstances>::Element *E=g.items.front();E;E=E->next()) {
 
 		Octant::ItemInstances &ii=E->get();
@@ -609,6 +645,7 @@ void GridMap::_octant_update(const OctantKey &p_key) {
 			xform.basis.scale(Vector3(cell_scale,cell_scale,cell_scale));
 
 			ii.multimesh->set_instance_transform(idx,xform);
+			//ii.multimesh->set_instance_transform(idx,Transform()	);
 			ii.multimesh->set_instance_color(idx,Color(1,1,1,1));
 			//print_line("MMINST: "+xform);
 
@@ -624,8 +661,11 @@ void GridMap::_octant_update(const OctantKey &p_key) {
 			if (ii.shape.is_valid()) {
 
 				PhysicsServer::get_singleton()->body_add_shape(g.static_body,ii.shape->get_rid(),xform);
-			//	print_line("PHIS x: "+xform);
+				if (g.collision_debug.is_valid()) {
+					ii.shape->add_vertices_to_array(col_debug,xform);
+				}
 
+			//	print_line("PHIS x: "+xform);
 			}
 
 			idx++;
@@ -636,6 +676,20 @@ void GridMap::_octant_update(const OctantKey &p_key) {
 
 	}
 
+	if (col_debug.size()) {
+
+
+		Array arr;
+		arr.resize(VS::ARRAY_MAX);
+		arr[VS::ARRAY_VERTEX]=col_debug;
+
+		VS::get_singleton()->mesh_add_surface(g.collision_debug,VS::PRIMITIVE_LINES,arr);
+		SceneTree *st=SceneTree::get_singleton();
+		if (st) {
+			VS::get_singleton()->mesh_surface_set_material( g.collision_debug, 0,st->get_debug_collision_material()->get_rid() );
+		}
+	}
+
 	g.dirty=false;
 
 }
@@ -656,6 +710,12 @@ void GridMap::_octant_exit_world(const OctantKey &p_key) {
 
 	}
 
+	if (g.collision_debug_instance.is_valid()) {
+
+		VS::get_singleton()->instance_set_room(g.collision_debug_instance,RID());
+		VS::get_singleton()->instance_set_scenario(g.collision_debug_instance,RID());
+	}
+
 	for(Map<int,Octant::ItemInstances>::Element *E=g.items.front();E;E=E->next()) {
 
 		VS::get_singleton()->instance_set_scenario(E->get().multimesh_instance,RID());
@@ -959,6 +1019,11 @@ void GridMap::_clear_internal(bool p_keep_areas) {
 		if (E->get()->bake_instance.is_valid())
 			VS::get_singleton()->free(E->get()->bake_instance);
 
+		if (E->get()->collision_debug.is_valid())
+			VS::get_singleton()->free(E->get()->collision_debug);
+		if (E->get()->collision_debug_instance.is_valid())
+			VS::get_singleton()->free(E->get()->collision_debug_instance);
+
 		PhysicsServer::get_singleton()->free(E->get()->static_body);
 		memdelete(E->get());
 
diff --git a/modules/gridmap/grid_map.h b/modules/gridmap/grid_map.h
index dff9fe3d68..9d3b1dcf95 100644
--- a/modules/gridmap/grid_map.h
+++ b/modules/gridmap/grid_map.h
@@ -93,6 +93,8 @@ class GridMap : public Spatial {
 
 		Ref<Mesh> baked;
 		RID bake_instance;
+		RID collision_debug;
+		RID collision_debug_instance;
 
 		bool dirty;
 		RID static_body;