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

9 files changed, 207 insertions, 130 deletions

diff --git a/scene/2d/canvas_item.cpp b/scene/2d/canvas_item.cpp
index 92d5088b81..49229ba500 100644
--- a/scene/2d/canvas_item.cpp
+++ b/scene/2d/canvas_item.cpp
@@ -1172,6 +1172,14 @@ Matrix32 CanvasItem::get_viewport_transform() const {
 }
 
 
+void CanvasItem::set_notify_local_transform(bool p_enable) {
+	notify_local_transform=p_enable;
+}
+
+bool CanvasItem::is_local_transform_notification_enabled() const {
+	return notify_local_transform;
+}
+
 CanvasItem::CanvasItem() : xform_change(this) {
 
 
@@ -1191,6 +1199,7 @@ CanvasItem::CanvasItem() : xform_change(this) {
 	canvas_layer=NULL;
 	use_parent_material=false;
 	global_invalid=true;
+	notify_local_transform=false;
 	light_mask=1;
 
 	C=NULL;
diff --git a/scene/2d/canvas_item.h b/scene/2d/canvas_item.h
index 6d8308dbe4..4885256c64 100644
--- a/scene/2d/canvas_item.h
+++ b/scene/2d/canvas_item.h
@@ -126,6 +126,7 @@ private:
 	bool block_transform_notify;
 	bool behind;
 	bool use_parent_material;
+	bool notify_local_transform;
 
 	Ref<CanvasItemMaterial> material;
 
@@ -155,7 +156,7 @@ private:
 
 protected:
 
-	_FORCE_INLINE_ void _notify_transform() { if (!is_inside_tree()) return; _notify_transform(this); if (!block_transform_notify) notification(NOTIFICATION_LOCAL_TRANSFORM_CHANGED); }
+	_FORCE_INLINE_ void _notify_transform() { if (!is_inside_tree()) return; _notify_transform(this); if (!block_transform_notify && notify_local_transform) notification(NOTIFICATION_LOCAL_TRANSFORM_CHANGED); }
 
 	void item_rect_changed();
 
@@ -263,6 +264,10 @@ public:
 	Vector2 get_global_mouse_pos() const;
 	Vector2 get_local_mouse_pos() const;
 
+	void set_notify_local_transform(bool p_enable);
+	bool is_local_transform_notification_enabled() const;
+
+
 	CanvasItem();
 	~CanvasItem();
 };
diff --git a/scene/2d/collision_polygon_2d.cpp b/scene/2d/collision_polygon_2d.cpp
index ceea41d1c8..1479cb7881 100644
--- a/scene/2d/collision_polygon_2d.cpp
+++ b/scene/2d/collision_polygon_2d.cpp
@@ -33,7 +33,7 @@
 
 void CollisionPolygon2D::_add_to_collision_object(Object *p_obj) {
 
-	if (unparenting)
+	if (unparenting || !can_update_body)
 		return;
 
 	CollisionObject2D *co = p_obj->cast_to<CollisionObject2D>();
@@ -49,6 +49,7 @@ void CollisionPolygon2D::_add_to_collision_object(Object *p_obj) {
 		//here comes the sun, lalalala
 		//decompose concave into multiple convex polygons and add them
 		Vector< Vector<Vector2> > decomp = Geometry::decompose_polygon(polygon);
+		shape_from=co->get_shape_count();
 		for(int i=0;i<decomp.size();i++) {
 			Ref<ConvexPolygonShape2D> convex = memnew( ConvexPolygonShape2D );
 			convex->set_points(decomp[i]);
@@ -57,6 +58,11 @@ void CollisionPolygon2D::_add_to_collision_object(Object *p_obj) {
 				co->set_shape_as_trigger(co->get_shape_count()-1,true);
 
 		}
+		shape_to=co->get_shape_count()-1;
+		if (shape_to<shape_from) {
+			shape_from=-1;
+			shape_to=-1;
+		}
 
 	} else {
 
@@ -78,6 +84,9 @@ void CollisionPolygon2D::_add_to_collision_object(Object *p_obj) {
 		if (trigger)
 			co->set_shape_as_trigger(co->get_shape_count()-1,true);
 
+		shape_from=co->get_shape_count()-1;
+		shape_to=co->get_shape_count()-1;
+
 	}
 
 
@@ -86,6 +95,8 @@ void CollisionPolygon2D::_add_to_collision_object(Object *p_obj) {
 
 void CollisionPolygon2D::_update_parent() {
 
+	if (!can_update_body)
+		return;
 	Node *parent = get_parent();
 	if (!parent)
 		return;
@@ -101,33 +112,55 @@ void CollisionPolygon2D::_notification(int p_what) {
 	switch(p_what) {
 		case NOTIFICATION_ENTER_TREE: {
 			unparenting=false;
+			can_update_body=get_tree()->is_editor_hint();
+		} break;
+		case NOTIFICATION_EXIT_TREE: {
+			can_update_body=false;
 		} break;
 		case NOTIFICATION_LOCAL_TRANSFORM_CHANGED: {
 
 			if (!is_inside_tree())
 				break;
-			_update_parent();
+			if (can_update_body) {
+				_update_parent();
+			} else if (shape_from>=0 && shape_to>=0) {
+				CollisionObject2D *co = get_parent()->cast_to<CollisionObject2D>();
+				for(int i=shape_from;i<=shape_to;i++) {
+					co->set_shape_transform(i,get_transform());
+				}
+			}
+
 
 		} break;
 
 		case NOTIFICATION_DRAW: {
+
+			if (!get_tree()->is_editor_hint() && !get_tree()->is_debugging_collisions_hint()) {
+				break;
+			}
+
+
 			for(int i=0;i<polygon.size();i++) {
 
 				Vector2 p = polygon[i];
 				Vector2 n = polygon[(i+1)%polygon.size()];
-				draw_line(p,n,Color(0,0.6,0.7,0.5),3);
+				draw_line(p,n,Color(0.9,0.2,0.0,0.8),3);
 			}
+//#define DEBUG_DECOMPOSE
+#if defined(TOOLS_ENABLED) && defined (DEBUG_DECOMPOSE)
 
 			Vector< Vector<Vector2> > decomp = Geometry::decompose_polygon(polygon);
-#define DEBUG_DECOMPOSE
-#ifdef DEBUG_DECOMPOSE
 			Color c(0.4,0.9,0.1);
 			for(int i=0;i<decomp.size();i++) {
 
 				c.set_hsv( Math::fmod(c.get_h() + 0.738,1),c.get_s(),c.get_v(),0.5);
 				draw_colored_polygon(decomp[i],c);
 			}
+#else
+			draw_colored_polygon(polygon,get_tree()->get_debug_collisions_color());
 #endif
+
+
 		} break;
 		case NOTIFICATION_UNPARENTED: {
 			unparenting = true;
@@ -141,20 +174,22 @@ void CollisionPolygon2D::set_polygon(const Vector<Point2>& p_polygon) {
 
 	polygon=p_polygon;
 
-	for(int i=0;i<polygon.size();i++) {
-		if (i==0)
-			aabb=Rect2(polygon[i],Size2());
-		else
-			aabb.expand_to(polygon[i]);
-	}
-	if (aabb==Rect2()) {
+	if (can_update_body) {
+		for(int i=0;i<polygon.size();i++) {
+			if (i==0)
+				aabb=Rect2(polygon[i],Size2());
+			else
+				aabb.expand_to(polygon[i]);
+		}
+		if (aabb==Rect2()) {
 
-		aabb=Rect2(-10,-10,20,20);
-	} else {
-		aabb.pos-=aabb.size*0.3;
-		aabb.size+=aabb.size*0.6;
+			aabb=Rect2(-10,-10,20,20);
+		} else {
+			aabb.pos-=aabb.size*0.3;
+			aabb.size+=aabb.size*0.6;
+		}
+		_update_parent();
 	}
-	_update_parent();
 	update();
 }
 
@@ -184,6 +219,13 @@ void CollisionPolygon2D::set_trigger(bool p_trigger) {
 
 	trigger=p_trigger;
 	_update_parent();
+	if (!can_update_body && is_inside_tree() && shape_from>=0 && shape_to>=0) {
+		CollisionObject2D *co = get_parent()->cast_to<CollisionObject2D>();
+		for(int i=shape_from;i<=shape_to;i++) {
+			co->set_shape_as_trigger(i,p_trigger);
+		}
+
+	}
 }
 
 bool CollisionPolygon2D::is_trigger() const{
@@ -192,6 +234,17 @@ bool CollisionPolygon2D::is_trigger() const{
 }
 
 
+void CollisionPolygon2D::_set_shape_range(const Vector2& p_range) {
+
+	shape_from=p_range.x;
+	shape_to=p_range.y;
+}
+
+Vector2 CollisionPolygon2D::_get_shape_range() const {
+
+	return Vector2(shape_from,shape_to);
+}
+
 void CollisionPolygon2D::_bind_methods() {
 
 	ObjectTypeDB::bind_method(_MD("_add_to_collision_object"),&CollisionPolygon2D::_add_to_collision_object);
@@ -204,9 +257,17 @@ void CollisionPolygon2D::_bind_methods() {
 	ObjectTypeDB::bind_method(_MD("set_trigger"),&CollisionPolygon2D::set_trigger);
 	ObjectTypeDB::bind_method(_MD("is_trigger"),&CollisionPolygon2D::is_trigger);
 
+	ObjectTypeDB::bind_method(_MD("_set_shape_range","shape_range"),&CollisionPolygon2D::_set_shape_range);
+	ObjectTypeDB::bind_method(_MD("_get_shape_range"),&CollisionPolygon2D::_get_shape_range);
+
+	ObjectTypeDB::bind_method(_MD("get_collision_object_first_shape"),&CollisionPolygon2D::get_collision_object_first_shape);
+	ObjectTypeDB::bind_method(_MD("get_collision_object_last_shape"),&CollisionPolygon2D::get_collision_object_last_shape);
+
 	ADD_PROPERTY( PropertyInfo(Variant::INT,"build_mode",PROPERTY_HINT_ENUM,"Solids,Segments"),_SCS("set_build_mode"),_SCS("get_build_mode"));
 	ADD_PROPERTY( PropertyInfo(Variant::VECTOR2_ARRAY,"polygon"),_SCS("set_polygon"),_SCS("get_polygon"));
+	ADD_PROPERTY( PropertyInfo(Variant::VECTOR2,"shape_range",PROPERTY_HINT_NONE,"",PROPERTY_USAGE_NOEDITOR),_SCS("_set_shape_range"),_SCS("_get_shape_range"));
 	ADD_PROPERTY( PropertyInfo(Variant::BOOL,"trigger"),_SCS("set_trigger"),_SCS("is_trigger"));
+
 }
 
 CollisionPolygon2D::CollisionPolygon2D() {
@@ -215,6 +276,9 @@ CollisionPolygon2D::CollisionPolygon2D() {
 	build_mode=BUILD_SOLIDS;
 	trigger=false;
 	unparenting=false;
-
+	shape_from=-1;
+	shape_to=-1;
+	can_update_body=false;
+	set_notify_local_transform(true);
 
 }
diff --git a/scene/2d/collision_polygon_2d.h b/scene/2d/collision_polygon_2d.h
index 4e78868082..4bc9713c8a 100644
--- a/scene/2d/collision_polygon_2d.h
+++ b/scene/2d/collision_polygon_2d.h
@@ -56,6 +56,14 @@ protected:
 	void _add_to_collision_object(Object *p_obj);
 	void _update_parent();
 
+	bool can_update_body;
+	int shape_from;
+	int shape_to;
+
+	void _set_shape_range(const Vector2& p_range);
+	Vector2 _get_shape_range() const;
+
+
 protected:
 
 	void _notification(int p_what);
@@ -72,6 +80,10 @@ public:
 	Vector<Point2> get_polygon() const;
 
 	virtual Rect2 get_item_rect() const;
+
+	int get_collision_object_first_shape() const { return shape_from; }
+	int get_collision_object_last_shape() const { return shape_to; }
+
 	CollisionPolygon2D();
 };
 
diff --git a/scene/2d/collision_shape_2d.cpp b/scene/2d/collision_shape_2d.cpp
index 02e4c79a08..cd89e914fb 100644
--- a/scene/2d/collision_shape_2d.cpp
+++ b/scene/2d/collision_shape_2d.cpp
@@ -44,10 +44,12 @@ void CollisionShape2D::_add_to_collision_object(Object *p_obj) {
 
 	CollisionObject2D *co = p_obj->cast_to<CollisionObject2D>();
 	ERR_FAIL_COND(!co);
+	update_shape_index=co->get_shape_count();
 	co->add_shape(shape,get_transform());
 	if (trigger)
 		co->set_shape_as_trigger(co->get_shape_count()-1,true);
 
+
 }
 
 void CollisionShape2D::_shape_changed() {
@@ -74,12 +76,27 @@ void CollisionShape2D::_notification(int p_what) {
 
 		case NOTIFICATION_ENTER_TREE: {
 			unparenting=false;
+			can_update_body=get_tree()->is_editor_hint();
+
 		} break;
 		case NOTIFICATION_LOCAL_TRANSFORM_CHANGED: {
 
 			if (!is_inside_tree())
 				break;
-			_update_parent();
+			if (can_update_body) {
+				_update_parent();
+			} else if (update_shape_index>=0){
+
+				CollisionObject2D *co = get_parent()->cast_to<CollisionObject2D>();
+				if (co) {
+					co->set_shape_transform(update_shape_index,get_transform());
+				}
+
+			}
+
+		} break;
+		case NOTIFICATION_EXIT_TREE: {
+			can_update_body=false;
 
 		} break;
 		/*
@@ -92,110 +109,18 @@ void CollisionShape2D::_notification(int p_what) {
 		} break;*/
 		case NOTIFICATION_DRAW: {
 
-			if (!get_tree()->is_editor_hint() && !get_tree()->is_debugging_collisions_hint())
+			if (!get_tree()->is_editor_hint() && !get_tree()->is_debugging_collisions_hint()) {
 				break;
+			}
 
 			rect=Rect2();
 
 
-			Color draw_col=Color(0,0.6,0.7,0.5);
-
-			if (shape->cast_to<LineShape2D>()) {
-
-				LineShape2D *l = shape->cast_to<LineShape2D>();
-				Vector2 point = l->get_d() * l->get_normal();
-
-				Vector2 l1[2]={point-l->get_normal().tangent()*100,point+l->get_normal().tangent()*100};
-				draw_line(l1[0],l1[1],draw_col,3);
-				Vector2 l2[2]={point,point+l->get_normal()*30};
-				draw_line(l2[0],l2[1],draw_col,3);
-				rect.pos=l1[0];
-				rect.expand_to(l1[1]);
-				rect.expand_to(l2[0]);
-				rect.expand_to(l2[1]);
-
-			} else if (shape->cast_to<SegmentShape2D>()) {
-
-				SegmentShape2D *s = shape->cast_to<SegmentShape2D>();
-				draw_line(s->get_a(),s->get_b(),draw_col,3);
-				rect.pos=s->get_a();
-				rect.expand_to(s->get_b());
-
-			} else if (shape->cast_to<RayShape2D>()) {
-
-				RayShape2D *s = shape->cast_to<RayShape2D>();
-
-				Vector2 tip = Vector2(0,s->get_length());
-				draw_line(Vector2(),tip,draw_col,3);
-				Vector<Vector2> pts;
-				float tsize=4;
-				pts.push_back(tip+Vector2(0,tsize));
-				pts.push_back(tip+Vector2(0.707*tsize,0));
-				pts.push_back(tip+Vector2(-0.707*tsize,0));
-				Vector<Color> cols;
-				for(int i=0;i<3;i++)
-					cols.push_back(draw_col);
-
-				draw_primitive(pts,cols,Vector<Vector2>()); //small arrow
-
-				rect.pos=Vector2();
-				rect.expand_to(tip);
-				rect=rect.grow(0.707*tsize);
-
-			} else if (shape->cast_to<CircleShape2D>()) {
-
-				CircleShape2D *s = shape->cast_to<CircleShape2D>();
-				Vector<Vector2> points;
-				for(int i=0;i<24;i++) {
-
-					points.push_back(Vector2(Math::cos(i*Math_PI*2/24.0),Math::sin(i*Math_PI*2/24.0))*s->get_radius());
-				}
-
-				draw_colored_polygon(points,draw_col);
-				rect.pos=-Point2(s->get_radius(),s->get_radius());
-				rect.size=Point2(s->get_radius(),s->get_radius())*2.0;
-
-			} else if (shape->cast_to<RectangleShape2D>()) {
-
-				RectangleShape2D *s = shape->cast_to<RectangleShape2D>();
-				Vector2 he = s->get_extents();
-				rect=Rect2(-he,he*2.0);
-				draw_rect(rect,draw_col);;
-
-			} else if (shape->cast_to<CapsuleShape2D>()) {
+			Color draw_col=get_tree()->get_debug_collisions_color();
+			shape->draw(get_canvas_item(),draw_col);
 
-				CapsuleShape2D *s = shape->cast_to<CapsuleShape2D>();
-
-				Vector<Vector2> points;
-				for(int i=0;i<24;i++) {
-					Vector2 ofs = Vector2(0,(i>6 && i<=18) ? -s->get_height()*0.5 : s->get_height()*0.5);
-
-					points.push_back(Vector2(Math::sin(i*Math_PI*2/24.0),Math::cos(i*Math_PI*2/24.0))*s->get_radius() + ofs);
-					if (i==6 || i==18)
-						points.push_back(Vector2(Math::sin(i*Math_PI*2/24.0),Math::cos(i*Math_PI*2/24.0))*s->get_radius() - ofs);
-				}
-
-				draw_colored_polygon(points,draw_col);
-				Vector2 he=Point2(s->get_radius(),s->get_radius()+s->get_height()*0.5);
-				rect.pos=-he;
-				rect.size=he*2.0;
-
-			} else if (shape->cast_to<ConvexPolygonShape2D>()) {
-
-				ConvexPolygonShape2D *s = shape->cast_to<ConvexPolygonShape2D>();
-
-				Vector<Vector2> points = s->get_points();
-				for(int i=0;i<points.size();i++) {
-					if (i==0)
-						rect.pos=points[i];
-					else
-						rect.expand_to(points[i]);
-				}
-
-				draw_colored_polygon(points,draw_col);
-
-			}
 
+			rect=shape->get_rect();
 			rect=rect.grow(3);
 
 		} break;
@@ -213,7 +138,14 @@ void CollisionShape2D::set_shape(const Ref<Shape2D>& p_shape) {
 		shape->disconnect("changed",this,"_shape_changed");
 	shape=p_shape;
 	update();
-	_update_parent();
+	if (is_inside_tree() && can_update_body)
+		_update_parent();
+	if (is_inside_tree() && !can_update_body && update_shape_index>=0) {
+		CollisionObject2D *co = get_parent()->cast_to<CollisionObject2D>();
+		if (co) {
+			co->set_shape(update_shape_index,p_shape);
+		}
+	}
 	if (shape.is_valid())
 		shape->connect("changed",this,"_shape_changed");
 
@@ -232,7 +164,14 @@ Rect2 CollisionShape2D::get_item_rect() const {
 void CollisionShape2D::set_trigger(bool p_trigger) {
 
 	trigger=p_trigger;
-	_update_parent();
+	if (can_update_body) {
+		_update_parent();
+	} else if (is_inside_tree() && update_shape_index>=0){
+		CollisionObject2D *co = get_parent()->cast_to<CollisionObject2D>();
+		if (co) {
+			co->set_shape_as_trigger(update_shape_index,p_trigger);
+		}
+	}
 }
 
 bool CollisionShape2D::is_trigger() const{
@@ -240,6 +179,19 @@ bool CollisionShape2D::is_trigger() const{
 	return trigger;
 }
 
+
+void CollisionShape2D::_set_update_shape_index(int p_index) {
+
+
+	update_shape_index=p_index;
+}
+
+int CollisionShape2D::_get_update_shape_index() const{
+
+	return update_shape_index;
+}
+
+
 void CollisionShape2D::_bind_methods() {
 
 	ObjectTypeDB::bind_method(_MD("set_shape","shape"),&CollisionShape2D::set_shape);
@@ -249,14 +201,23 @@ void CollisionShape2D::_bind_methods() {
 	ObjectTypeDB::bind_method(_MD("set_trigger","enable"),&CollisionShape2D::set_trigger);
 	ObjectTypeDB::bind_method(_MD("is_trigger"),&CollisionShape2D::is_trigger);
 
+	ObjectTypeDB::bind_method(_MD("_set_update_shape_index","index"),&CollisionShape2D::_set_update_shape_index);
+	ObjectTypeDB::bind_method(_MD("_get_update_shape_index"),&CollisionShape2D::_get_update_shape_index);
+
+	ObjectTypeDB::bind_method(_MD("get_collision_object_shape_index"),&CollisionShape2D::get_collision_object_shape_index);
+
 	ADD_PROPERTYNZ(PropertyInfo(Variant::OBJECT,"shape",PROPERTY_HINT_RESOURCE_TYPE,"Shape2D"),_SCS("set_shape"),_SCS("get_shape"));
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL,"trigger"),_SCS("set_trigger"),_SCS("is_trigger"));
+	ADD_PROPERTY( PropertyInfo( Variant::INT, "_update_shape_index", PROPERTY_HINT_NONE, "",PROPERTY_USAGE_NOEDITOR), _SCS("_set_update_shape_index"), _SCS("_get_update_shape_index"));
+
 }
 
 CollisionShape2D::CollisionShape2D() {
 
 	rect=Rect2(-Point2(10,10),Point2(20,20));
-
+	set_notify_local_transform(true);
 	trigger=false;
 	unparenting = false;
+	can_update_body = false;
+	update_shape_index=-1;
 }
diff --git a/scene/2d/collision_shape_2d.h b/scene/2d/collision_shape_2d.h
index 507912d31e..82e1137174 100644
--- a/scene/2d/collision_shape_2d.h
+++ b/scene/2d/collision_shape_2d.h
@@ -39,7 +39,13 @@ class CollisionShape2D : public Node2D {
 	Rect2 rect;
 	bool trigger;
 	bool unparenting;
+	bool can_update_body;
 	void _shape_changed();
+	int update_shape_index;
+
+	void _set_update_shape_index(int p_index);
+	int _get_update_shape_index() const;
+
 protected:
 
 	void _update_parent();
@@ -55,6 +61,8 @@ public:
 	void set_trigger(bool p_trigger);
 	bool is_trigger() const;
 
+	int get_collision_object_shape_index() const { return _get_update_shape_index(); }
+
 	CollisionShape2D();
 };
 
diff --git a/scene/2d/navigation_polygon.cpp b/scene/2d/navigation_polygon.cpp
index fc69ea8a0d..792f079ab0 100644
--- a/scene/2d/navigation_polygon.cpp
+++ b/scene/2d/navigation_polygon.cpp
@@ -279,7 +279,7 @@ void NavigationPolygonInstance::set_enabled(bool p_enabled) {
 
 	}
 
-	if (get_tree()->is_editor_hint())
+	if (get_tree()->is_editor_hint() || get_tree()->is_debugging_navigation_hint())
 		update();
 
 //	update_gizmo();
@@ -338,7 +338,7 @@ void NavigationPolygonInstance::_notification(int p_what) {
 		} break;
 		case NOTIFICATION_DRAW: {
 
-			if (is_inside_tree() && get_tree()->is_editor_hint() && navpoly.is_valid()) {
+			if (is_inside_tree() && (get_tree()->is_editor_hint() || get_tree()->is_debugging_navigation_hint()) && navpoly.is_valid()) {
 
 				DVector<Vector2> verts=navpoly->get_vertices();
 				int vsize = verts.size();
@@ -348,9 +348,9 @@ void NavigationPolygonInstance::_notification(int p_what) {
 
 				Color color;
 				if (enabled) {
-					color=Color(0.1,0.8,1.0,0.4);
+					color=get_tree()->get_debug_navigation_color();
 				} else {
-					color=Color(1.0,0.8,0.1,0.4);
+					color=get_tree()->get_debug_navigation_disabled_color();
 				}
 				Vector<Color> colors;
 				Vector<Vector2> vertices;
@@ -423,7 +423,7 @@ Ref<NavigationPolygon> NavigationPolygonInstance::get_navigation_polygon() const
 
 void NavigationPolygonInstance::_navpoly_changed() {
 
-	if (is_inside_tree() && get_tree()->is_editor_hint())
+	if (is_inside_tree() && (get_tree()->is_editor_hint() || get_tree()->is_debugging_navigation_hint()))
 		update();
 }
 
diff --git a/scene/2d/ray_cast_2d.cpp b/scene/2d/ray_cast_2d.cpp
index 20abe42cd9..05594fd79c 100644
--- a/scene/2d/ray_cast_2d.cpp
+++ b/scene/2d/ray_cast_2d.cpp
@@ -115,17 +115,17 @@ void RayCast2D::_notification(int p_what) {
 				set_fixed_process(false);
 
 		} break;
-#ifdef TOOLS_ENABLED
+
 		case NOTIFICATION_DRAW: {
 
-			if (!get_tree()->is_editor_hint())
+			if (!get_tree()->is_editor_hint()  && !get_tree()->is_debugging_collisions_hint())
 				break;
 			Matrix32 xf;
 			xf.rotate(cast_to.atan2());
 			xf.translate(Vector2(0,cast_to.length()));
 
 			//Vector2 tip = Vector2(0,s->get_length());
-			Color dcol(0.9,0.2,0.2,0.4);
+			Color dcol=get_tree()->get_debug_collisions_color();//0.9,0.2,0.2,0.4);
 			draw_line(Vector2(),cast_to,dcol,3);
 			Vector<Vector2> pts;
 			float tsize=4;
@@ -139,7 +139,7 @@ void RayCast2D::_notification(int p_what) {
 			draw_primitive(pts,cols,Vector<Vector2>()); //small arrow
 
 		} break;
-#endif
+
 
 		case NOTIFICATION_FIXED_PROCESS: {
 
diff --git a/scene/2d/tile_map.cpp b/scene/2d/tile_map.cpp
index 17f93f816f..418ee192b2 100644
--- a/scene/2d/tile_map.cpp
+++ b/scene/2d/tile_map.cpp
@@ -30,6 +30,7 @@
 #include "io/marshalls.h"
 #include "servers/physics_2d_server.h"
 #include "method_bind_ext.inc"
+#include "os/os.h"
 
 int TileMap::_get_quadrant_size() const {
 
@@ -262,6 +263,14 @@ void TileMap::_update_dirty_quadrants() {
 
 	Vector2 qofs;
 
+	SceneTree *st=SceneTree::get_singleton();
+	Color debug_collision_color;
+
+	bool debug_shapes = st && st->is_debugging_collisions_hint();
+	if (debug_shapes) {
+		debug_collision_color=st->get_debug_collisions_color();
+	}
+
 	while (dirty_quadrant_list.first()) {
 
 		Quadrant &q = *dirty_quadrant_list.first()->self();
@@ -398,11 +407,19 @@ void TileMap::_update_dirty_quadrants() {
 
 					_fix_cell_transform(xform,c,shape_ofs+center_ofs,s);
 
-					ps->body_add_shape(q.body,shape->get_rid(),xform);
+					if (debug_shapes) {
+						vs->canvas_item_add_set_transform(canvas_item,xform);
+						shape->draw(canvas_item,debug_collision_color);
+
+					}
+					ps->body_add_shape(q.body,shape->get_rid(),xform);					
 					ps->body_set_shape_metadata(q.body,shape_idx++,Vector2(E->key().x,E->key().y));
 
 				}
 			}
+			if (debug_shapes) {
+				vs->canvas_item_add_set_transform(canvas_item,Matrix32());
+			}
 
 			if (navigation) {
 				Ref<NavigationPolygon> navpoly = tile_set->tile_get_navigation_polygon(c.id);
@@ -412,6 +429,7 @@ void TileMap::_update_dirty_quadrants() {
 					xform.set_origin(offset.floor()+q.pos);
 					_fix_cell_transform(xform,c,npoly_ofs+center_ofs,s);
 
+
 					int pid = navigation->navpoly_create(navpoly,nav_rel * xform);
 
 					Quadrant::NavPoly np;